/****************************************************************/
/*                                                              */
/*          Modified version of the                             */
/*          Advanced Navigation Packet Protocol Library         */
/*        ROS Driver, Packet to Published Message Example       */
/*          Copyright 2017, Advanced Navigation Pty Ltd         */
/*                                                              */
/****************************************************************/
/*
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

#include <ros/ros.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <unistd.h>

#include "serial/serial.h"
#include "an_packet_protocol.h"
#include "spatial_packets.h"

#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/TimeReference.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Twist.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <advanced_navigation_driver/RawSensor.h>
#include <advanced_navigation_driver/RawGNSS.h>

#define RADIANS_TO_DEGREES (180.0/M_PI)
#define SERIAL_TIMEOUT_MS 2000

int main(int argc, char *argv[]) {
    // Set up ROS node //
    ros::init(argc, argv, "an_device_node");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    printf("\nYour Advanced Navigation ROS driver is currently running\nPress Ctrl-C to interrupt\n");

    // Set up the COM port
    std::string com_port;
    int baud_rate;
    std::string imu_frame_id;
    std::string nav_sat_frame_id;
    std::string topic_prefix;

    if (argc >= 3) {
        com_port = std::string(argv[1]);
        baud_rate = atoi(argv[2]);
    }
    else {
        pnh.param("port", com_port, std::string("/dev/ttyUSB0"));
        pnh.param("baud_rate", baud_rate, 115200);
    }

    pnh.param("imu_frame_id", imu_frame_id, std::string("imu"));
    pnh.param("nav_sat_frame_id", nav_sat_frame_id, std::string("gps"));
    pnh.param("topic_prefix", topic_prefix, std::string("an_device"));

    // Initialise Publishers and Topics //
    ros::Publisher nav_sat_fix_pub=nh.advertise<sensor_msgs::NavSatFix>(topic_prefix + "/NavSatFix",10);
    ros::Publisher twist_pub=nh.advertise<geometry_msgs::Twist>(topic_prefix + "/Twist",10);
    ros::Publisher imu_pub=nh.advertise<sensor_msgs::Imu>(topic_prefix + "/Imu",10);
    ros::Publisher system_status_pub=nh.advertise<diagnostic_msgs::DiagnosticStatus>(topic_prefix + "/SystemStatus",10);
    ros::Publisher filter_status_pub=nh.advertise<diagnostic_msgs::DiagnosticStatus>(topic_prefix + "/FilterStatus",10);
    ros::Publisher odometry_pub=nh.advertise<nav_msgs::Odometry>(topic_prefix + "/Odometry",10);
    ros::Publisher rawimu_pub=nh.advertise<advanced_navigation_driver::RawSensor>(topic_prefix + "/RawImu",10);
    ros::Publisher rawgnss_pub=nh.advertise<advanced_navigation_driver::RawGNSS>(topic_prefix + "/RawGNSS",10);

    // Initialise messages
    sensor_msgs::NavSatFix nav_sat_fix_msg;
    nav_sat_fix_msg.header.stamp.sec=0;
    nav_sat_fix_msg.header.stamp.nsec=0;
    nav_sat_fix_msg.header.frame_id='0';
    nav_sat_fix_msg.status.status=0;
    nav_sat_fix_msg.status.service=1; // fixed to GPS
    nav_sat_fix_msg.latitude=0.0;
    nav_sat_fix_msg.longitude=0.0;
    nav_sat_fix_msg.altitude=0.0;
    nav_sat_fix_msg.position_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
    nav_sat_fix_msg.position_covariance_type=2; // fixed to variance on the diagonal

    geometry_msgs::Twist twist_msg;
    twist_msg.linear.x=0.0;
    twist_msg.linear.y=0.0;
    twist_msg.linear.z=0.0;
    twist_msg.angular.x=0.0;
    twist_msg.angular.y=0.0;
    twist_msg.angular.z=0.0;

    sensor_msgs::Imu imu_msg;
    imu_msg.header.stamp.sec=0;
    imu_msg.header.stamp.nsec=0;
    imu_msg.header.frame_id='0';
    imu_msg.orientation.x=0.0;
    imu_msg.orientation.y=0.0;
    imu_msg.orientation.z=0.0;
    imu_msg.orientation.w=0.0;
    imu_msg.orientation_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
    imu_msg.angular_velocity.x=0.0;
    imu_msg.angular_velocity.y=0.0;
    imu_msg.angular_velocity.z=0.0;
    imu_msg.angular_velocity_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed
    imu_msg.linear_acceleration.x=0.0;
    imu_msg.linear_acceleration.y=0.0;
    imu_msg.linear_acceleration.z=0.0;
    imu_msg.linear_acceleration_covariance={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; // fixed

    diagnostic_msgs::DiagnosticStatus system_status_msg;
    system_status_msg.level = 0; // default OK state
    system_status_msg.name = "System Status";
    system_status_msg.message = "";

    diagnostic_msgs::DiagnosticStatus filter_status_msg;
    filter_status_msg.level = 0; // default OK state
    filter_status_msg.name = "Filter Status";
    filter_status_msg.message = "";

    nav_msgs::Odometry odometry_msg;
    odometry_msg.header.stamp.sec = 0;
    odometry_msg.header.stamp.nsec = 0;
    odometry_msg.header.frame_id = '0'; // TODO: wgs84;
    odometry_msg.child_frame_id = 3; // TODO: 'ins';
    odometry_msg.pose.covariance = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                        0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    odometry_msg.pose.pose.position.x = 0.0;
    odometry_msg.pose.pose.position.y = 0.0;
    odometry_msg.pose.pose.position.z = 0.0;
    odometry_msg.pose.pose.orientation.x = 0.0;
    odometry_msg.pose.pose.orientation.y = 0.0;
    odometry_msg.pose.pose.orientation.z = 0.0;
    odometry_msg.pose.pose.orientation.w = 0.0;

    advanced_navigation_driver::RawSensor rawimu_msg;
    rawimu_msg.accelerometer.x=0.0;
    rawimu_msg.accelerometer.y=0.0;
    rawimu_msg.accelerometer.z=0.0;
    rawimu_msg.gyroscope.x=0.0;
    rawimu_msg.gyroscope.y=0.0;
    rawimu_msg.gyroscope.z=0.0;
    rawimu_msg.magnetometer.x=0.0;
    rawimu_msg.magnetometer.y=0.0;
    rawimu_msg.magnetometer.z=0.0;
    rawimu_msg.imuTempC=0.0;
    rawimu_msg.pressurePascals=0.0;
    rawimu_msg.pressureTempC=0.0;

    advanced_navigation_driver::RawGNSS rawgnss_msg;
    rawgnss_msg.header.stamp.sec=0;
    rawgnss_msg.header.stamp.nsec=0;
    rawgnss_msg.header.frame_id='0';
    rawgnss_msg.latitudeRad=0.0;
    rawgnss_msg.longitudeRad=0.0;
    rawgnss_msg.heightM=0.0;
    rawgnss_msg.velocityN=0.0;
    rawgnss_msg.velocityE=0.0;
    rawgnss_msg.velocityD=0.0;
    rawgnss_msg.latitudeStdDev=0.0;
    rawgnss_msg.longitudeStdDev=0.0;
    rawgnss_msg.heightStdDev=0.0;
    rawgnss_msg.reserved0=0.0;
    rawgnss_msg.reserved1=0.0;
    rawgnss_msg.reserved2=0.0;
    rawgnss_msg.reserved3=0.0;
    rawgnss_msg.status = 0;

    // get data from com port //
    an_decoder_t an_decoder;
    an_packet_t *an_packet;
    system_state_packet_t system_state_packet;
    quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet;
    raw_sensors_packet_t raw_sensors_packet;
    raw_gnss_packet_t raw_gnss_packet;
    int bytes_received;

    serial::Serial serialPort(com_port, baud_rate, serial::Timeout::simpleTimeout(SERIAL_TIMEOUT_MS));

    if (!serialPort.isOpen())
    {
        printf("Could not open serial port: %s \n",com_port.c_str());
        exit(EXIT_FAILURE);
    }

    an_decoder_initialise(&an_decoder);

    // Loop continuously, polling for packets
    while (ros::ok())
    {
        ros::spinOnce();
        if ((bytes_received = serialPort.read(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
        {
            // increment the decode buffer length by the number of bytes received //
            an_decoder_increment(&an_decoder, bytes_received);

            // decode all the packets in the buffer //
            while ((an_packet = an_packet_decode(&an_decoder)) != NULL)
            {
                // system state packet //
                if (an_packet->id == packet_id_system_state)
                {
                    if(decode_system_state_packet(&system_state_packet, an_packet) == 0)
                    {
                        // NavSatFix
                        nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds;
                        nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000;
                        nav_sat_fix_msg.header.frame_id=nav_sat_frame_id;
                        if ((system_state_packet.filter_status.b.gnss_fix_type == 1) ||
                            (system_state_packet.filter_status.b.gnss_fix_type == 2))
                        {
                            nav_sat_fix_msg.status.status=0;
                        }
                        else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) ||
                             (system_state_packet.filter_status.b.gnss_fix_type == 5))
                        {
                            nav_sat_fix_msg.status.status=1;
                        }
                        else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) ||
                             (system_state_packet.filter_status.b.gnss_fix_type == 6) ||
                             (system_state_packet.filter_status.b.gnss_fix_type == 7))
                        {
                            nav_sat_fix_msg.status.status=2;
                        }
                        else
                        {
                            nav_sat_fix_msg.status.status=-1;
                        }
                        nav_sat_fix_msg.latitude=system_state_packet.latitude * RADIANS_TO_DEGREES;
                        nav_sat_fix_msg.longitude=system_state_packet.longitude * RADIANS_TO_DEGREES;
                        nav_sat_fix_msg.altitude=system_state_packet.height;
                        nav_sat_fix_msg.position_covariance={pow(system_state_packet.standard_deviation[1],2), 0.0, 0.0,
                            0.0, pow(system_state_packet.standard_deviation[0],2), 0.0,
                            0.0, 0.0, pow(system_state_packet.standard_deviation[2],2)};

                        // Twist
                        twist_msg.linear.x=system_state_packet.velocity[0];
                        twist_msg.linear.y=system_state_packet.velocity[1];
                        twist_msg.linear.z=system_state_packet.velocity[2];
                        twist_msg.angular.x=system_state_packet.angular_velocity[0];
                        twist_msg.angular.y=system_state_packet.angular_velocity[1];
                        twist_msg.angular.z=system_state_packet.angular_velocity[2];

                        // IMU
                        imu_msg.header.stamp.sec=system_state_packet.unix_time_seconds;
                        imu_msg.header.stamp.nsec=system_state_packet.microseconds*1000;
                        imu_msg.header.frame_id=imu_frame_id;
                        // Convert roll, pitch, yaw from radians to quaternion format //
                        float phi = system_state_packet.orientation[0] / 2.0f;
                        float theta = system_state_packet.orientation[1] / 2.0f;
                        float psi = system_state_packet.orientation[2] / 2.0f;
                        float sin_phi = sinf(phi);
                        float cos_phi = cosf(phi);
                        float sin_theta = sinf(theta);
                        float cos_theta = cosf(theta);
                        float sin_psi = sinf(psi);
                        float cos_psi = cosf(psi);
                        imu_msg.orientation.x=-cos_phi * sin_theta * sin_psi + sin_phi * cos_theta * cos_psi;
                        imu_msg.orientation.y=cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi;
                        imu_msg.orientation.z=cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi;
                        imu_msg.orientation.w=cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi;

                        imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values
                        imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1];
                        imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2];
                        imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0];
                        imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1];
                        imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2];

                        // System Status
                        system_status_msg.message = "";
                        system_status_msg.level = 0; // default OK state
                        if (system_state_packet.system_status.b.system_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "0. System Failure! ";
                        }
                        if (system_state_packet.system_status.b.accelerometer_sensor_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "1. Accelerometer Sensor Failure! ";
                        }
                        if (system_state_packet.system_status.b.gyroscope_sensor_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "2. Gyroscope Sensor Failure! ";
                        }
                        if (system_state_packet.system_status.b.magnetometer_sensor_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "3. Magnetometer Sensor Failure! ";
                        }
                        if (system_state_packet.system_status.b.pressure_sensor_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "4. Pressure Sensor Failure! ";
                        }
                        if (system_state_packet.system_status.b.gnss_failure) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "5. GNSS Failure! ";
                        }
                        if (system_state_packet.system_status.b.accelerometer_over_range) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "6. Accelerometer Over Range! ";
                        }
                        if (system_state_packet.system_status.b.gyroscope_over_range) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "7. Gyroscope Over Range! ";
                        }
                        if (system_state_packet.system_status.b.magnetometer_over_range) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "8. Magnetometer Over Range! ";
                        }
                        if (system_state_packet.system_status.b.pressure_over_range) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "9. Pressure Over Range! ";
                        }
                        if (system_state_packet.system_status.b.minimum_temperature_alarm) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "10. Minimum Temperature Alarm! ";
                        }
                        if (system_state_packet.system_status.b.maximum_temperature_alarm) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "11. Maximum Temperature Alarm! ";
                        }
                        if (system_state_packet.system_status.b.low_voltage_alarm) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "12. Low Voltage Alarm! ";
                        }
                        if (system_state_packet.system_status.b.high_voltage_alarm) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "13. High Voltage Alarm! ";
                        }
                        if (system_state_packet.system_status.b.gnss_antenna_disconnected) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "14. GNSS Antenna Disconnected! ";
                        }
                        if (system_state_packet.system_status.b.serial_port_overflow_alarm) {
                            system_status_msg.level = 2; // ERROR state
                            system_status_msg.message = system_status_msg.message + "15. Data Output Overflow Alarm! ";
                        }

                        // Filter Status
                        filter_status_msg.message = "";
                        filter_status_msg.level = 0; // default OK state
                        if (system_state_packet.filter_status.b.orientation_filter_initialised) {
                            filter_status_msg.message = filter_status_msg.message + "0. Orientation Filter Initialised. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "0. Orientation Filter NOT Initialised. ";
                        }
                        if (system_state_packet.filter_status.b.ins_filter_initialised) {
                            filter_status_msg.message = filter_status_msg.message + "1. Navigation Filter Initialised. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "1. Navigation Filter NOT Initialised. ";
                        }
                        if (system_state_packet.filter_status.b.heading_initialised) {
                            filter_status_msg.message = filter_status_msg.message + "2. Heading Initialised. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "2. Heading NOT Initialised. ";
                        }
                        if (system_state_packet.filter_status.b.utc_time_initialised) {
                            filter_status_msg.message = filter_status_msg.message + "3. UTC Time Initialised. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "3. UTC Time NOT Initialised. ";
                        }
                        if (system_state_packet.filter_status.b.event1_flag) {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "7. Event 1 Occured. ";
                        }
                        else {
                            filter_status_msg.message = filter_status_msg.message + "7. Event 1 NOT Occured. ";
                        }
                        if (system_state_packet.filter_status.b.event2_flag) {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "8. Event 2 Occured. ";
                        }
                        else {
                            filter_status_msg.message = filter_status_msg.message + "8. Event 2 NOT Occured. ";
                        }
                        if (system_state_packet.filter_status.b.internal_gnss_enabled) {
                            filter_status_msg.message = filter_status_msg.message + "9. Internal GNSS Enabled. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "9. Internal GNSS NOT Enabled. ";
                        }
                        if (system_state_packet.filter_status.b.magnetic_heading_enabled) {
                            filter_status_msg.message = filter_status_msg.message + "10. Magnetic Heading Active. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "10. Magnetic Heading NOT Active. ";
                        }
                        if (system_state_packet.filter_status.b.velocity_heading_enabled) {
                            filter_status_msg.message = filter_status_msg.message + "11. Velocity Heading Enabled. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "11. Velocity Heading NOT Enabled. ";
                        }
                        if (system_state_packet.filter_status.b.atmospheric_altitude_enabled) {
                            filter_status_msg.message = filter_status_msg.message + "12. Atmospheric Altitude Enabled. ";
                        }
                        else {
                            filter_status_msg.message = filter_status_msg.message + "12. Atmospheric Altitude NOT Enabled. ";
                            filter_status_msg.level = 1; // WARN state
                        }
                        if (system_state_packet.filter_status.b.external_position_active) {
                            filter_status_msg.message = filter_status_msg.message + "13. External Position Active. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "13. External Position NOT Active. ";
                        }
                        if (system_state_packet.filter_status.b.external_velocity_active) {
                            filter_status_msg.message = filter_status_msg.message + "14. External Velocity Active. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "14. External Velocity NOT Active. ";
                        }
                        if (system_state_packet.filter_status.b.external_heading_active) {
                            filter_status_msg.message = filter_status_msg.message + "15. External Heading Active. ";
                        }
                        else {
                            filter_status_msg.level = 1; // WARN state
                            filter_status_msg.message = filter_status_msg.message + "15. External Heading NOT Active. ";
                        }

                        // Odometry
                        odometry_msg.header.stamp.sec=system_state_packet.unix_time_seconds;
                        odometry_msg.header.stamp.nsec=system_state_packet.microseconds*1000;
                        odometry_msg.pose.pose.position.y=system_state_packet.latitude * RADIANS_TO_DEGREES;
                        odometry_msg.pose.pose.position.x=system_state_packet.longitude * RADIANS_TO_DEGREES;
                        odometry_msg.pose.pose.position.z=system_state_packet.height;
                        odometry_msg.pose.covariance[0] = pow(system_state_packet.standard_deviation[1],2);
                        odometry_msg.pose.covariance[7] = pow(system_state_packet.standard_deviation[0],2);
                        odometry_msg.pose.covariance[14] = pow(system_state_packet.standard_deviation[2],2);
                        // TODO: see if we can use these?:
                        //odometry_msg.pose.covariance[21] = pow(euler_orientation_standard_deviation_packet[0],2);
                        //odometry_msg.pose.covariance[28] = pow(euler_orientation_standard_deviation_packet[1],2);
                        //odometry_msg.pose.covariance[35] = pow(euler_orientation_standard_deviation_packet[2],2);
                        odometry_msg.pose.covariance[21] = pow(quaternion_orientation_standard_deviation_packet.standard_deviation[0],2);
                        odometry_msg.pose.covariance[28] = pow(quaternion_orientation_standard_deviation_packet.standard_deviation[1],2);
                        odometry_msg.pose.covariance[35] = pow(quaternion_orientation_standard_deviation_packet.standard_deviation[2],2);
                        odometry_msg.pose.pose.orientation.x = imu_msg.orientation.x;
                        odometry_msg.pose.pose.orientation.y = imu_msg.orientation.y;
                        odometry_msg.pose.pose.orientation.z = imu_msg.orientation.z;
                        odometry_msg.pose.pose.orientation.w = imu_msg.orientation.w;
                    }
                }

                // quaternion orientation standard deviation packet //
                if (an_packet->id == packet_id_quaternion_orientation_standard_deviation)
                {
                    // copy all the binary data into the typedef struct for the packet //
                    // this allows easy access to all the different values             //
                    if(decode_quaternion_orientation_standard_deviation_packet(&quaternion_orientation_standard_deviation_packet, an_packet) == 0)
                    {
                        // IMU
                        imu_msg.orientation_covariance[0] = quaternion_orientation_standard_deviation_packet.standard_deviation[0];
                        imu_msg.orientation_covariance[4] = quaternion_orientation_standard_deviation_packet.standard_deviation[1];
                        imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2];
                    }
                }

                // raw sensor packet
                if (an_packet->id == packet_id_raw_sensors)
                {
                    if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0)
                    {
                        rawimu_msg.accelerometer.x=raw_sensors_packet.accelerometers[0];
                        rawimu_msg.accelerometer.y=raw_sensors_packet.accelerometers[1];
                        rawimu_msg.accelerometer.z=raw_sensors_packet.accelerometers[2];
                        rawimu_msg.gyroscope.x=raw_sensors_packet.gyroscopes[0];
                        rawimu_msg.gyroscope.y=raw_sensors_packet.gyroscopes[1];
                        rawimu_msg.gyroscope.z=raw_sensors_packet.gyroscopes[2];
                        rawimu_msg.magnetometer.x=raw_sensors_packet.magnetometers[0];
                        rawimu_msg.magnetometer.y=raw_sensors_packet.magnetometers[1];
                        rawimu_msg.magnetometer.z=raw_sensors_packet.magnetometers[2];
                        rawimu_msg.imuTempC=raw_sensors_packet.imu_temperature;
                        rawimu_msg.pressurePascals=raw_sensors_packet.pressure;
                        rawimu_msg.pressureTempC=raw_sensors_packet.pressure_temperature;
                    }
                }

                // raw gnss packet
                if (an_packet->id == packet_id_raw_gnss)
                {
                    if(decode_raw_gnss_packet(&raw_gnss_packet, an_packet) == 0)
                    {
                        rawgnss_msg.header.stamp.sec=raw_gnss_packet.unix_time_seconds;
                        rawgnss_msg.header.stamp.nsec=raw_gnss_packet.microseconds*1000;
                        rawgnss_msg.latitudeRad=raw_gnss_packet.position[0];
                        rawgnss_msg.longitudeRad=raw_gnss_packet.position[1];
                        rawgnss_msg.heightM=raw_gnss_packet.position[2];
                        rawgnss_msg.velocityN=raw_gnss_packet.velocity[0];
                        rawgnss_msg.velocityE=raw_gnss_packet.velocity[1];
                        rawgnss_msg.velocityD=raw_gnss_packet.velocity[2];
                        rawgnss_msg.latitudeStdDev=raw_gnss_packet.position_standard_deviation[0];
                        rawgnss_msg.longitudeStdDev=raw_gnss_packet.position_standard_deviation[1];
                        rawgnss_msg.heightStdDev=raw_gnss_packet.position_standard_deviation[2];
                        rawgnss_msg.status = raw_gnss_packet.flags.r;
                    }
                }

                // Ensure that you free the an_packet when your done with it //
                // or you will leak memory                                   //
                an_packet_free(&an_packet);

                // Publish messages //
                nav_sat_fix_pub.publish(nav_sat_fix_msg);
                twist_pub.publish(twist_msg);
                imu_pub.publish(imu_msg);
                system_status_pub.publish(system_status_msg);
                filter_status_pub.publish(filter_status_msg);
                odometry_pub.publish(odometry_msg);
                rawimu_pub.publish(rawimu_msg);
                rawgnss_pub.publish(rawgnss_msg);
            }
        }
    }

}

