diff --git a/libraries/SITL/SIM_AdNav.cpp b/libraries/SITL/SIM_AdNav.cpp
new file mode 100644
index 00000000000000..8442eea60b0218
--- /dev/null
+++ b/libraries/SITL/SIM_AdNav.cpp
@@ -0,0 +1,513 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ Simulation for a Advanced Navigation External AHRS System
+*/
+
+#include "SIM_AdNav.h"
+
+using namespace SITL;
+
+AdNav::AdNav() :
+ SerialDevice::SerialDevice()
+{
+ an_decoder_initialise(&_an_decoder);
+}
+/*
+ get timeval using simulation time
+ */
+static void simulation_timeval(struct timeval *tv)
+{
+ uint64_t now = AP_HAL::micros64();
+ static uint64_t first_usec;
+ static struct timeval first_tv;
+ if (first_usec == 0) {
+ first_usec = now;
+ first_tv.tv_sec = AP::sitl()->start_time_UTC;
+ }
+ *tv = first_tv;
+ tv->tv_sec += now / 1000000ULL;
+ uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
+ tv->tv_sec += new_usec / 1000000ULL;
+ tv->tv_usec = new_usec % 1000000ULL;
+}
+
+void AdNav::update()
+{
+ if(!init_sitl_pointer()) {
+ return;
+ }
+
+ // Receive Packets
+ receive_packets();
+
+ // Get Current Time
+ uint32_t now = AP_HAL::micros();
+
+ // If we need to send packets do so now
+ if (now - _last_pkt_sent_us >= _packet_period_us) {
+ _last_pkt_sent_us = now;
+ send_state_pkt(); // Send ANPP.20
+ send_vel_sd_pkt(); // Send ANPP.25
+ send_raw_sensors_pkt(); // Send ANPP.28
+ send_raw_gnss_pkt(); // Send ANPP.29
+ send_sat_pkt(); // Send ANPP.30
+ }
+}
+
+void AdNav::receive_packets(){
+ ssize_t bytes_received = 0;
+ an_packet_t* an_packet;
+
+ if ((bytes_received = read_from_autopilot((char*) an_decoder_pointer(&_an_decoder), an_decoder_size(&_an_decoder))) > 0)
+ {
+ an_decoder_increment(&_an_decoder, bytes_received);
+
+ while((an_packet = an_packet_decode(&_an_decoder)) != NULL)
+ {
+ switch(an_packet->id){
+ // If we have been sent a periods packet
+ case AN_PACKET_ID_PACKET_PERIODS:
+ send_acknowledge(an_packet_crc(an_packet), AN_PACKET_ID_PACKET_PERIODS);
+ packet_periods_packet_t packet_periods;
+ decode_packet_periods_packet(&packet_periods, an_packet);
+ _packet_period_us = packet_periods.packet_periods->period * 1.0e3;
+ break;
+ case AN_PACKET_ID_REQUEST_PACKET:
+ // This is usually where any packet ID can be requested
+ // However only ID:3 Device info is ever requested by the module
+ // So this will return a Device info packet
+ send_device_info_pkt();
+ break;
+ default:
+ printf("AN_DEVICE_SIM: Unknown AN Packet");
+ break;
+ }
+ }
+ }
+}
+
+void AdNav::send_packet(an_packet_t* an_packet){
+ // Encode the packet.
+ an_packet_encode(an_packet);
+
+ // Write the packet to the autopilot
+ write_to_autopilot((char *) an_packet_pointer(an_packet), an_packet_size(an_packet));
+
+ // Free the packet memory
+ an_packet_free(&an_packet);
+}
+
+/*
+ Function to send an Acknowledgement Packet.
+ CRC - CRC of the packet being acknowledged
+ ID - ID of the packet being acknowledged
+ */
+void AdNav::send_acknowledge(uint16_t crc, uint8_t id){
+ acknowledge_packet_t ack_pkt;
+
+ memset(&ack_pkt, 0, sizeof(ack_pkt));
+ ack_pkt.acknowledge_result = 0; // success
+ ack_pkt.packet_crc = crc;
+ ack_pkt.packet_id = id;
+
+ send_packet(encode_acknowledge_packet(&ack_pkt));
+}
+
+/*
+ Function to send a Device Info Packet.
+ */
+void AdNav::send_device_info_pkt(){
+ device_information_packet_t dev_info;
+
+ memset(&dev_info, 0, sizeof(dev_info));
+ dev_info.device_id = 26; // Certus
+
+ send_packet(encode_device_information_packet(&dev_info));
+}
+
+/*
+ Function to send a System State Packet.
+ */
+void AdNav::send_state_pkt(){
+ const auto &fdm = _sitl->state;
+
+ system_state_packet_t system_state_packet;
+ memset(&system_state_packet, 0, sizeof(system_state_packet));
+
+ system_state_packet.system_status.r = false; // no AN Device Errors
+ system_state_packet.filter_status.b.orientation_filter_initialised = true;
+ system_state_packet.filter_status.b.ins_filter_initialised = true;
+ system_state_packet.filter_status.b.heading_initialised = true;
+ system_state_packet.filter_status.b.gnss_fix_type = 7; // RTK Fixed
+ system_state_packet.latitude = radians(fdm.latitude);
+ system_state_packet.longitude = radians(fdm.longitude);
+ system_state_packet.height = fdm.altitude;
+ system_state_packet.velocity[0] = fdm.speedN;
+ system_state_packet.velocity[1] = fdm.speedE;
+ system_state_packet.velocity[2] = fdm.speedD;
+ system_state_packet.body_acceleration[0] = fdm.xAccel;
+ system_state_packet.body_acceleration[1] = fdm.yAccel;
+ system_state_packet.body_acceleration[2] = fdm.zAccel;
+ system_state_packet.g_force = 1; // Unused
+ system_state_packet.orientation[0] = radians(fdm.rollDeg);
+ system_state_packet.orientation[1] = radians(fdm.pitchDeg);
+ system_state_packet.orientation[2] = radians(fdm.heading);
+ system_state_packet.angular_velocity[0] = radians(fdm.rollRate);
+ system_state_packet.angular_velocity[1] = radians(fdm.pitchRate);
+ system_state_packet.angular_velocity[2] = radians(fdm.yawRate);
+ // fdm doesn't contain SD values for LLH
+
+ send_packet(encode_system_state_packet(&system_state_packet));
+}
+
+/*
+ Function to send a Velocity Standard Deviation Packet.
+ */
+void AdNav::send_vel_sd_pkt(){
+ // FDM Does not contain any Velocity SD Information so send 0's instead.
+ velocity_standard_deviation_packet_t vel_sd;
+ memset(&vel_sd, 0, sizeof(vel_sd));
+ send_packet(encode_velocity_standard_deviation_packet(&vel_sd));
+
+}
+
+/*
+ Function to send a Raw Sensors Packet.
+ */
+void AdNav::send_raw_sensors_pkt(){
+ const auto& fdm = _sitl->state;
+
+ raw_sensors_packet_t raw_sensors;
+ memset(&raw_sensors, 0, sizeof(raw_sensors));
+
+ raw_sensors.accelerometers[0] = fdm.xAccel;
+ raw_sensors.accelerometers[1] = fdm.yAccel;
+ raw_sensors.accelerometers[2] = fdm.zAccel;
+ raw_sensors.gyroscopes[0] = radians(fdm.rollRate);
+ raw_sensors.gyroscopes[1] = radians(fdm.pitchRate);
+ raw_sensors.gyroscopes[2] = radians(fdm.yawRate);
+ raw_sensors.magnetometers[0] = fdm.bodyMagField[0];
+ raw_sensors.magnetometers[1] = fdm.bodyMagField[1];
+ raw_sensors.magnetometers[2] = fdm.bodyMagField[2];
+ raw_sensors.imu_temperature = 25; //fixed
+
+ float sigma, delta, theta;
+ AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta);
+ raw_sensors.pressure = SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.01; // 500 ft fixed
+ raw_sensors.pressure_temperature = 25; // fixed
+
+ send_packet(encode_raw_sensors_packet(&raw_sensors));
+}
+
+/*
+ Function to send a Raw GNSS Packet.
+ */
+void AdNav::send_raw_gnss_pkt(){
+ const auto& fdm = _sitl->state;
+
+ raw_gnss_packet_t raw_gnss;
+ memset(&raw_gnss, 0, sizeof(raw_gnss));
+
+ struct timeval tv;
+ simulation_timeval(&tv);
+
+
+ raw_gnss.unix_time_seconds = tv.tv_sec;
+ raw_gnss.microseconds = tv.tv_usec;
+ raw_gnss.position[0] = radians(fdm.latitude);
+ raw_gnss.position[1] = radians(fdm.longitude);
+ raw_gnss.position[2] = fdm.altitude;
+ raw_gnss.velocity[0] = fdm.speedN;
+ raw_gnss.velocity[1] = fdm.speedE;
+ raw_gnss.velocity[2] = fdm.speedD;
+ raw_gnss.heading = radians(fdm.heading);
+ raw_gnss.flags.b.fix_type = 7; // rtk fixed
+ raw_gnss.position_standard_deviation[0] = 0.8;
+ raw_gnss.position_standard_deviation[1] = 0.8;
+ raw_gnss.position_standard_deviation[2] = 1.2;
+
+ send_packet(encode_raw_gnss_packet(&raw_gnss));
+}
+
+/*
+ Function to send a Raw Satellites Packet.
+ */
+void AdNav::send_sat_pkt(){
+ satellites_packet_t sat_pkt;
+ memset(&sat_pkt, 0, sizeof(sat_pkt));
+
+ // Values taken from a GNSS Compass in Newcastle AU.
+ sat_pkt.hdop = 1.1;
+ sat_pkt.vdop = 1.2;
+ sat_pkt.beidou_satellites = 8;
+ sat_pkt.galileo_satellites = 6;
+ sat_pkt.gps_satellites = 7;
+ sat_pkt.sbas_satellites = 3;
+
+ send_packet(encode_satellites_packet(&sat_pkt));
+}
+
+/*
+ Function to encode an Acknowledge Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_acknowledge_packet(acknowledge_packet_t* acknowledge_packet){
+ an_packet_t* an_packet = an_packet_allocate(4, AN_PACKET_ID_ACKNOWLEDGE);
+ if(an_packet != NULL){
+ memcpy(&an_packet->data[0], &acknowledge_packet->packet_id, sizeof(uint8_t));
+ memcpy(&an_packet->data[1], &acknowledge_packet->packet_crc, sizeof(uint16_t));
+ memcpy(&an_packet->data[3], &acknowledge_packet->acknowledge_result, sizeof(uint8_t));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a Device Information Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_device_information_packet(device_information_packet_t* device_information_packet){
+ an_packet_t* an_packet = an_packet_allocate(24, AN_PACKET_ID_DEVICE_INFO);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &device_information_packet->software_version, sizeof(uint32_t));
+ memcpy(&an_packet->data[4], &device_information_packet->device_id, sizeof(uint32_t));
+ memcpy(&an_packet->data[8], &device_information_packet->hardware_revision, sizeof(uint32_t));
+ memcpy(&an_packet->data[12], &device_information_packet->serial_number[0], 3 * sizeof(uint32_t));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a System State Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_system_state_packet(system_state_packet_t* system_state_packet){
+ an_packet_t* an_packet = an_packet_allocate(100, AN_PACKET_ID_SYSTEM_STATE);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &system_state_packet->system_status, sizeof(uint16_t));
+ memcpy(&an_packet->data[2], &system_state_packet->filter_status, sizeof(uint16_t));
+ memcpy(&an_packet->data[4], &system_state_packet->unix_time_seconds, sizeof(uint32_t));
+ memcpy(&an_packet->data[8], &system_state_packet->microseconds, sizeof(uint32_t));
+ memcpy(&an_packet->data[12], &system_state_packet->latitude, sizeof(double));
+ memcpy(&an_packet->data[20], &system_state_packet->longitude, sizeof(double));
+ memcpy(&an_packet->data[28], &system_state_packet->height, sizeof(double));
+ memcpy(&an_packet->data[36], &system_state_packet->velocity[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[48], &system_state_packet->body_acceleration[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[60], &system_state_packet->g_force, sizeof(float));
+ memcpy(&an_packet->data[64], &system_state_packet->orientation[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[76], &system_state_packet->angular_velocity[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[88], &system_state_packet->standard_deviation[0], 3 * sizeof(float));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a Velocity Standard Deviation Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_velocity_standard_deviation_packet(velocity_standard_deviation_packet_t* velocity_standard_deviation_packet){
+ an_packet_t* an_packet = an_packet_allocate(12, AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &velocity_standard_deviation_packet->standard_deviation[0], 3 * sizeof(float));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a Raw Sensors Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_raw_sensors_packet(raw_sensors_packet_t* raw_sensors_packet){
+ an_packet_t* an_packet = an_packet_allocate(48, AN_PACKET_ID_RAW_SENSORS);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &raw_sensors_packet->accelerometers[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[12], &raw_sensors_packet->gyroscopes[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[24], &raw_sensors_packet->magnetometers[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[36], &raw_sensors_packet->imu_temperature, sizeof(float));
+ memcpy(&an_packet->data[40], &raw_sensors_packet->pressure, sizeof(float));
+ memcpy(&an_packet->data[44], &raw_sensors_packet->pressure_temperature, sizeof(float));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a Raw GNSS Packet into a an_packet.
+ */
+an_packet_t* AdNav::encode_raw_gnss_packet(raw_gnss_packet_t* raw_gnss_packet){
+ an_packet_t* an_packet = an_packet_allocate(74, AN_PACKET_ID_RAW_GNSS);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &raw_gnss_packet->unix_time_seconds, sizeof(uint32_t));
+ memcpy(&an_packet->data[4], &raw_gnss_packet->microseconds, sizeof(uint32_t));
+ memcpy(&an_packet->data[8], &raw_gnss_packet->position[0], 3 * sizeof(double));
+ memcpy(&an_packet->data[32], &raw_gnss_packet->velocity[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[44], &raw_gnss_packet->position_standard_deviation[0], 3 * sizeof(float));
+ memcpy(&an_packet->data[56], &raw_gnss_packet->tilt, sizeof(float));
+ memcpy(&an_packet->data[60], &raw_gnss_packet->heading, sizeof(float));
+ memcpy(&an_packet->data[64], &raw_gnss_packet->tilt_standard_deviation, sizeof(float));
+ memcpy(&an_packet->data[68], &raw_gnss_packet->heading_standard_deviation, sizeof(float));
+ memcpy(&an_packet->data[72], &raw_gnss_packet->flags.r, sizeof(uint16_t));
+ }
+ return an_packet;
+}
+
+/*
+ Function to encode a Satellites Packet into an an_packet structure.
+ */
+an_packet_t* AdNav::encode_satellites_packet(satellites_packet_t* satellites_packet){
+ an_packet_t* an_packet = an_packet_allocate(13, AN_PACKET_ID_SATELLITES);
+ if (an_packet != NULL)
+ {
+ memcpy(&an_packet->data[0], &satellites_packet->hdop, sizeof(float));
+ memcpy(&an_packet->data[4], &satellites_packet->vdop, sizeof(float));
+ memcpy(&an_packet->data[8], &satellites_packet->gps_satellites, 5 * sizeof(uint8_t));
+ }
+ return an_packet;
+}
+
+/*
+ Function to decode a Packet Periods Packet into an an_packet structure.
+ */
+int AdNav::decode_packet_periods_packet(packet_periods_packet_t* packet_periods_packet, an_packet_t* an_packet){
+ if(an_packet->id == AN_PACKET_ID_PACKET_PERIODS && (an_packet->length - 2) % 5 == 0)
+ {
+ int i;
+ int packet_periods_count = (an_packet->length - 2) / 5;
+ packet_periods_packet->permanent = an_packet->data[0];
+ packet_periods_packet->clear_existing_packets = an_packet->data[1];
+ for(i = 0; i < AN_MAXIMUM_PACKET_PERIODS; i++)
+ {
+ if(i < packet_periods_count)
+ {
+ packet_periods_packet->packet_periods[i].packet_id = an_packet->data[2 + 5 * i];
+ memcpy(&packet_periods_packet->packet_periods[i].period, &an_packet->data[2 + 5 * i + 1], sizeof(uint32_t));
+ }
+ else memset(&packet_periods_packet->packet_periods[i], 0, sizeof(packet_period_t));
+ }
+ return 0;
+ }
+ else return 1;
+}
+
+/*
+ * Function to calculate a 4 byte LRC
+ */
+uint8_t AdNav::calculate_header_lrc(uint8_t* data)
+{
+ return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1;
+}
+
+/*
+ * Function to dynamically allocate an an_packet
+ */
+an_packet_t* AdNav::an_packet_allocate(uint8_t length, uint8_t id)
+{
+ an_packet_t* an_packet = (an_packet_t*) malloc(sizeof(an_packet_t) + length * sizeof(uint8_t));
+ if (an_packet != NULL) {
+ an_packet->id = id;
+ an_packet->length = length;
+ }
+ return an_packet;
+}
+
+/*
+ * Function to free an an_packet
+ */
+void AdNav::an_packet_free(an_packet_t** an_packet)
+{
+ free(*an_packet);
+ *an_packet = NULL;
+}
+
+/*
+ * Initialise the decoder
+ */
+void AdNav::an_decoder_initialise(an_decoder_t* an_decoder)
+{
+ an_decoder->buffer_length = 0;
+ an_decoder->packets_decoded = 0;
+ an_decoder->bytes_decoded = 0;
+ an_decoder->bytes_discarded = 0;
+ an_decoder->lrc_errors = 0;
+ an_decoder->crc_errors = 0;
+}
+
+/*
+ * Function to decode an_packets from raw data
+ * Returns a pointer to the packet decoded or NULL if no packet was decoded
+ */
+an_packet_t* AdNav::an_packet_decode(an_decoder_t* an_decoder)
+{
+ uint16_t decode_iterator = 0;
+ an_packet_t* an_packet = NULL;
+ uint8_t header_lrc, id, length;
+ uint16_t crc;
+
+ while (decode_iterator + AN_PACKET_HEADER_SIZE <= an_decoder->buffer_length) {
+ header_lrc = an_decoder->buffer[decode_iterator++];
+ if (header_lrc == calculate_header_lrc(&an_decoder->buffer[decode_iterator])) {
+ id = an_decoder->buffer[decode_iterator++];
+ length = an_decoder->buffer[decode_iterator++];
+ crc = an_decoder->buffer[decode_iterator++];
+ crc |= an_decoder->buffer[decode_iterator++] << 8;
+
+ if (decode_iterator + length > an_decoder->buffer_length) {
+ decode_iterator -= AN_PACKET_HEADER_SIZE;
+ break;
+ }
+
+ if (crc == crc16_ccitt(&an_decoder->buffer[decode_iterator], length, 0xFFFF)) {
+ an_packet = an_packet_allocate(length, id);
+ if (an_packet != NULL) {
+ memcpy(an_packet->header, &an_decoder->buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE * sizeof(uint8_t));
+ memcpy(an_packet->data, &an_decoder->buffer[decode_iterator], length * sizeof(uint8_t));
+ }
+ decode_iterator += length;
+ an_decoder->packets_decoded++;
+ an_decoder->bytes_decoded += length + AN_PACKET_HEADER_SIZE;
+ break;
+ } else {
+ decode_iterator -= (AN_PACKET_HEADER_SIZE - 1);
+ an_decoder->crc_errors++;
+ an_decoder->bytes_discarded++;
+ }
+ } else {
+ an_decoder->lrc_errors++;
+ an_decoder->bytes_discarded++;
+ }
+ }
+ if (decode_iterator < an_decoder->buffer_length) {
+ if (decode_iterator > 0) {
+ memmove(&an_decoder->buffer[0], &an_decoder->buffer[decode_iterator], (an_decoder->buffer_length - decode_iterator) * sizeof(uint8_t));
+ an_decoder->buffer_length -= decode_iterator;
+ }
+ } else {
+ an_decoder->buffer_length = 0;
+ }
+
+ return an_packet;
+}
+
+/*
+ * Function to encode an an_packet
+ */
+void AdNav::an_packet_encode(an_packet_t* an_packet)
+{
+ uint16_t crc;
+ an_packet->header[1] = an_packet->id;
+ an_packet->header[2] = an_packet->length;
+ crc = crc16_ccitt(an_packet->data, an_packet->length, 0xFFFF);
+ memcpy(&an_packet->header[3], &crc, sizeof(uint16_t));
+ an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]);
+}
\ No newline at end of file
diff --git a/libraries/SITL/SIM_AdNav.h b/libraries/SITL/SIM_AdNav.h
new file mode 100644
index 00000000000000..0721c2af0a17fc
--- /dev/null
+++ b/libraries/SITL/SIM_AdNav.h
@@ -0,0 +1,270 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+/*
+ Simulation for a Advanced Navigation External AHRS System.
+ usage:
+ PARAMS:
+ param set AHRS_EKF_TYPE 11
+ param set EAHRS_TYPE 3
+ param set SERIAL5_PROTOCOL 36
+ param set SERIAL5_BAUD 115
+
+ sim_vehicle.py -D --console --map -A --serial5=sim:AdNav
+ */
+
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+#include
+#include
+#include "SIM_SerialDevice.h"
+#include
+
+#define AN_PACKET_ID_PACKET_PERIODS 181
+#define AN_PACKET_ID_SATELLITES 30
+#define AN_PACKET_ID_RAW_GNSS 29
+#define AN_PACKET_ID_RAW_SENSORS 28
+#define AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION 25
+#define AN_PACKET_ID_SYSTEM_STATE 20
+#define AN_PACKET_ID_DEVICE_INFO 3
+#define AN_PACKET_ID_REQUEST_PACKET 1
+#define AN_PACKET_ID_ACKNOWLEDGE 0
+#define AN_PACKET_HEADER_SIZE 5
+#define AN_MAXIMUM_PACKET_SIZE 255
+#define AN_DECODE_BUFFER_SIZE 2*(AN_MAXIMUM_PACKET_SIZE+AN_PACKET_HEADER_SIZE)
+#define AN_GPS_EPOCH_UNIX_OFFSET 315964800 // GPS Week 0 sec 0 is midnight Sunday Jan 6th 1980 UTC
+#define AN_TIMEOUT 5000 //ms
+#define AN_MAXIMUM_PACKET_PERIODS 50
+
+#define an_packet_pointer(packet) packet->header
+#define an_packet_size(packet) (packet->length + AN_PACKET_HEADER_SIZE)*sizeof(uint8_t)
+#define an_packet_crc(packet) ((packet->header[4]<<8) | packet->header[3])
+
+#define an_decoder_pointer(an_decoder) &(an_decoder)->buffer[(an_decoder)->buffer_length]
+#define an_decoder_size(an_decoder) (sizeof((an_decoder)->buffer) - (an_decoder)->buffer_length)
+#define an_decoder_increment(an_decoder, bytes_received) (an_decoder)->buffer_length += bytes_received
+
+typedef struct {
+ uint8_t id;
+ uint8_t length;
+ uint8_t header[AN_PACKET_HEADER_SIZE];
+ uint8_t data[1];
+} an_packet_t;
+
+typedef enum
+{
+ acknowledge_success,
+ acknowledge_failure_crc,
+ acknowledge_failure_length,
+ acknowledge_failure_range,
+ acknowledge_failure_flash,
+ acknowledge_failure_not_ready,
+ acknowledge_failure_unknown_packet
+} acknowledge_result_e;
+
+typedef struct
+{
+ uint8_t packet_id;
+ uint16_t packet_crc;
+ uint8_t acknowledge_result;
+} acknowledge_packet_t;
+
+typedef struct {
+ uint32_t software_version;
+ uint32_t device_id;
+ uint32_t hardware_revision;
+ uint32_t serial_number[3];
+} device_information_packet_t;
+
+
+typedef struct {
+ uint8_t buffer[AN_DECODE_BUFFER_SIZE];
+ uint16_t buffer_length;
+ uint64_t packets_decoded;
+ uint64_t bytes_decoded;
+ uint64_t bytes_discarded;
+ uint64_t lrc_errors;
+ uint64_t crc_errors;
+} an_decoder_t;
+
+typedef struct {
+ union {
+ uint16_t r;
+ struct {
+ uint16_t system_failure :1;
+ uint16_t accelerometer_sensor_failure :1;
+ uint16_t gyroscope_sensor_failure :1;
+ uint16_t magnetometer_sensor_failure :1;
+ uint16_t pressure_sensor_failure :1;
+ uint16_t gnss_failure :1;
+ uint16_t accelerometer_over_range :1;
+ uint16_t gyroscope_over_range :1;
+ uint16_t magnetometer_over_range :1;
+ uint16_t pressure_over_range :1;
+ uint16_t minimum_temperature_alarm :1;
+ uint16_t maximum_temperature_alarm :1;
+ uint16_t internal_data_logging_error :1;
+ uint16_t high_voltage_alarm :1;
+ uint16_t gnss_antenna_fault :1;
+ uint16_t serial_port_overflow_alarm :1;
+ } b;
+ } system_status;
+ union {
+ uint16_t r;
+ struct {
+ uint16_t orientation_filter_initialised :1;
+ uint16_t ins_filter_initialised :1;
+ uint16_t heading_initialised :1;
+ uint16_t utc_time_initialised :1;
+ uint16_t gnss_fix_type :3;
+ uint16_t event1_flag :1;
+ uint16_t event2_flag :1;
+ uint16_t internal_gnss_enabled :1;
+ uint16_t dual_antenna_heading_active :1;
+ uint16_t velocity_heading_enabled :1;
+ uint16_t atmospheric_altitude_enabled :1;
+ uint16_t external_position_active :1;
+ uint16_t external_velocity_active :1;
+ uint16_t external_heading_active :1;
+ } b;
+ } filter_status;
+ uint32_t unix_time_seconds;
+ uint32_t microseconds;
+ double latitude;
+ double longitude;
+ double height;
+ float velocity[3];
+ float body_acceleration[3];
+ float g_force;
+ float orientation[3];
+ float angular_velocity[3];
+ float standard_deviation[3];
+} system_state_packet_t;
+
+typedef struct {
+ float standard_deviation[3];
+} velocity_standard_deviation_packet_t;
+
+typedef struct {
+ float accelerometers[3];
+ float gyroscopes[3];
+ float magnetometers[3];
+ float imu_temperature;
+ float pressure;
+ float pressure_temperature;
+} raw_sensors_packet_t;
+
+typedef struct {
+ uint32_t unix_time_seconds;
+ uint32_t microseconds;
+ double position[3];
+ float velocity[3];
+ float position_standard_deviation[3];
+ float tilt; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ float heading; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ float tilt_standard_deviation; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ float heading_standard_deviation; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ union {
+ uint16_t r;
+ struct {
+ uint16_t fix_type :3;
+ uint16_t velocity_valid :1;
+ uint16_t time_valid :1;
+ uint16_t external_gnss :1;
+ uint16_t tilt_valid :1; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ uint16_t heading_valid :1; /* This field will only be valid if an external dual antenna GNSS system is connected */
+ } b;
+ } flags;
+} raw_gnss_packet_t;
+
+typedef struct {
+ float hdop;
+ float vdop;
+ uint8_t gps_satellites;
+ uint8_t glonass_satellites;
+ uint8_t beidou_satellites;
+ uint8_t galileo_satellites;
+ uint8_t sbas_satellites;
+} satellites_packet_t;
+
+typedef struct {
+ uint8_t packet_id;
+ uint32_t period;
+} packet_period_t;
+
+typedef struct {
+ uint8_t permanent;
+ uint8_t clear_existing_packets;
+ packet_period_t packet_periods[AN_MAXIMUM_PACKET_PERIODS];
+} packet_periods_packet_t;
+
+
+
+
+namespace SITL
+{
+
+class AdNav : public SerialDevice
+{
+public:
+
+ AdNav();
+
+ // update state
+ void update(void);
+
+private:
+ an_decoder_t _an_decoder;
+
+ uint32_t _packet_period_us = 20000; // Period to send packets.
+
+ uint32_t _last_pkt_sent_us;
+
+ void receive_packets();
+ void send_packet(an_packet_t* an_packet);
+ void send_acknowledge(uint16_t crc, uint8_t id);
+ void send_device_info_pkt();
+ void send_state_pkt();
+ void send_vel_sd_pkt();
+ void send_raw_sensors_pkt();
+ void send_raw_gnss_pkt();
+ void send_sat_pkt();
+
+ uint64_t start_us;
+
+ an_packet_t* encode_acknowledge_packet(acknowledge_packet_t* acknowledge_packet);
+ an_packet_t* encode_device_information_packet(device_information_packet_t* device_information_packet);
+ an_packet_t* encode_system_state_packet(system_state_packet_t* system_state_packet);
+ an_packet_t* encode_velocity_standard_deviation_packet(velocity_standard_deviation_packet_t* velocity_standard_deviation_packet);
+ an_packet_t* encode_raw_sensors_packet(raw_sensors_packet_t* raw_gnss_packet);
+ an_packet_t* encode_raw_gnss_packet(raw_gnss_packet_t* raw_gnss_packet);
+ an_packet_t* encode_satellites_packet(satellites_packet_t* satellites_packet);
+ int decode_packet_periods_packet(packet_periods_packet_t* packet_periods_packet, an_packet_t* an_packet);
+
+
+ uint8_t calculate_header_lrc(uint8_t* data);
+ an_packet_t* an_packet_allocate(uint8_t length, uint8_t id);
+ void an_packet_free(an_packet_t** an_packet);
+ void an_decoder_initialise(an_decoder_t* an_decoder);
+ an_packet_t* an_packet_decode(an_decoder_t* an_decoder);
+ void an_packet_encode(an_packet_t* an_packet);
+
+
+};
+
+}
+