diff --git a/Tools/autotest/ArduPlane_Tests/AdvancedNavigationEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/AdvancedNavigationEAHRS/ap1.txt new file mode 100644 index 0000000000000..ab2be0d1a4547 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/AdvancedNavigationEAHRS/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3ca7780e5ff45..dff6be1f583b0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3083,6 +3083,10 @@ def InertialLabsEAHRS(self): '''Test InertialLabs EAHRS support''' self.fly_external_AHRS("ILabs", 5, "ap1.txt") + def AdvancedNavigationEAHRS(self): + '''Test AdvancedNavigation EAHRS series 5 support''' + self.fly_external_AHRS("AdNav", 3, "ap1.txt") + def GpsSensorPreArmEAHRS(self): '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver''' self.customise_SITL_commandline(["--serial4=sim:MicroStrain7"]) @@ -6115,6 +6119,7 @@ def tests(self): self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, self.InertialLabsEAHRS, + self.AdvancedNavigationEAHRS, self.GpsSensorPreArmEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 229ca00dea23f..2441cfb2f0f58 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -26,6 +26,7 @@ #include "AP_ExternalAHRS_MicroStrain5.h" #include "AP_ExternalAHRS_MicroStrain7.h" #include "AP_ExternalAHRS_InertialLabs.h" +#include "AP_ExternalAHRS_AdvancedNavigation.h" #include #include @@ -73,6 +74,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @DisplayName: External AHRS options // @Description: External AHRS options bitmask // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag. + // @Bitmask: 1:Provide airspeed aiding to ExternalAHRS device from airspeed sensors. // @User: Standard AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0), @@ -89,7 +91,14 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Units: Hz // @User: Standard AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10), - + + // @Param: _ARSP_ERR_20MS + // @DisplayName: Airspeed error at 20m/s + // @Description: Estimate of error in airspeed when vehicle travelling at 20m/s + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("_ASER_20MS", 6, AP_ExternalAHRS, arsp_err_20ms, 1.0), + AP_GROUPEND }; @@ -130,6 +139,11 @@ void AP_ExternalAHRS::init(void) return; #endif +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + case DevType::AdNav: + backend = new AP_ExternalAHRS_AdvancedNavigation(this, state); + return; +#endif } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype)); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index a08c2822914b1..1e25a19915cee 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -34,6 +34,7 @@ class AP_ExternalAHRS { public: friend class AP_ExternalAHRS_backend; friend class AP_ExternalAHRS_VectorNav; + friend class AP_ExternalAHRS_AdvancedNavigation; AP_ExternalAHRS(); @@ -49,10 +50,12 @@ class AP_ExternalAHRS { #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED MicroStrain5 = 2, #endif +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + AdNav = 3, +#endif #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED InertialLabs = 5, #endif - // 3 reserved for AdNav // 4 reserved for CINS // 6 reserved for Trimble #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED @@ -173,6 +176,7 @@ class AP_ExternalAHRS { enum class OPTIONS { VN_UNCOMP_IMU = 1U << 0, + ARSP_AID = 1U << 1, }; bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; } @@ -184,6 +188,7 @@ class AP_ExternalAHRS { AP_Int16 log_rate; AP_Int16 options; AP_Int16 sensors; + AP_Float arsp_err_20ms; static AP_ExternalAHRS *_singleton; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp new file mode 100644 index 0000000000000..2f6cc97a6e113 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -0,0 +1,942 @@ +/* + 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 . + */ +/* + support for serial connected AHRS systems + */ + +#define ALLOW_DOUBLE_MATH_FUNCTIONS + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED +#include "AP_ExternalAHRS_AdvancedNavigation.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define AN_TIMEOUT 5000 //ms +#define AN_MAXIMUM_PACKET_PERIODS 50 +#define AN_GNSS_PACKET_RATE 5 +#define AN_AIRSPEED_AIDING_FREQUENCY 20 + +extern const AP_HAL::HAL &hal; + +/* + * Function to decode ANPP Packets from raw buffer in the decoder structure + * Returns false for a buffer error + */ +int AP_ExternalAHRS_AdvancedNavigation_Decoder::decode_packet(uint8_t* out_buffer, size_t buf_size) +{ + uint16_t decode_iterator = 0; + + // Iterate through buffer until no more headers could be in buffer + while (decode_iterator + AN_PACKET_HEADER_SIZE <= _buffer_length) { + const uint8_t header_lrc = _buffer[decode_iterator++]; + + // Is this the start of a valid header? + if (header_lrc == calculate_header_lrc(&_buffer[decode_iterator])) { + decode_iterator++; // skip ID as it is unused (-Werror=unused-but-set-variable) + const uint8_t length = _buffer[decode_iterator++]; + const uint16_t crc = _buffer[decode_iterator] | (_buffer[decode_iterator + 1] << 8); + decode_iterator += 2; + + // If the packet length is over the edge of the buffer + if (decode_iterator + length > _buffer_length) { + decode_iterator -= AN_PACKET_HEADER_SIZE; + break; + } + + // If the crc matches then a valid packet has been identified. + if (crc == crc16_ccitt(&_buffer[decode_iterator], length, 0xFFFF)) { + + // Protect from buffer overflow. + if ((size_t) (length + AN_PACKET_HEADER_SIZE) > buf_size) { + return false; + } + + // Save the data into the output buffer. + memcpy(out_buffer, &_buffer[decode_iterator - AN_PACKET_HEADER_SIZE], AN_PACKET_HEADER_SIZE + length * sizeof(uint8_t)); + + decode_iterator += length; + _packets_decoded++; + _bytes_decoded += length + AN_PACKET_HEADER_SIZE; + break; + } else { // Invalid packet for given header + decode_iterator -= (AN_PACKET_HEADER_SIZE - 1); + _crc_errors++; + _bytes_discarded++; + } + } else { // Invalid Header + _lrc_errors++; + _bytes_discarded++; + } + } + // If there is still buffer to be decoded. + if (decode_iterator < _buffer_length) { + // Ensure that the iterator isn't invalid + if (decode_iterator > 0) { + // move the unparsed memory to the beginning of the buffer. + memmove(&_buffer[0], &_buffer[decode_iterator], (_buffer_length - decode_iterator) * sizeof(uint8_t)); + _buffer_length -= decode_iterator; + _complete = false; + return true; + } + } else { + _buffer_length = 0; + } + + _complete = true; + return true; +} + + +// constructor +AP_ExternalAHRS_AdvancedNavigation::AP_ExternalAHRS_AdvancedNavigation(AP_ExternalAHRS *_frontend, + AP_ExternalAHRS::state_t &_state) : + AP_ExternalAHRS_backend(_frontend, _state), + dal (AP::dal()) +{ + auto &sm = AP::serialmanager(); + _uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); + if (_uart == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS no UART"); + return; + } + + _baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + _port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + _last_vel_sd = new AN_VELOCITY_STANDARD_DEVIATION; + _last_satellites = new AN_SATELLITES; + + // don't offer IMU by default, at 50Hz it is too slow + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_AdvancedNavigation::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + AP_HAL::panic("Failed to start ExternalAHRS update thread"); + } +} + +void AP_ExternalAHRS_AdvancedNavigation::update() +{ + get_packets(); +} + +void AP_ExternalAHRS_AdvancedNavigation::update_thread(void) +{ + _uart->begin(_baudrate); + + uint32_t tstart = AP_HAL::millis(); + + // Ensure device is responsive by requesting its information. + while (!_last_device_info_pkt_ms) + { + const uint32_t tnow = AP_HAL::millis(); + if (tnow - tstart >= AN_TIMEOUT) + { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Advanced Navigation Device Unresponsive"); + if (!request_data()) + { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Request Data Error"); + } + tstart = tnow; + } + + // Sleep the scheduler + hal.scheduler->delay_microseconds(1000); + + // Collect the requested packets from the UART manager + // This will still process all received packets like normal and feed data out. This ensures that it won't fail completely if the TX fails. + if (!get_packets()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Error Receiving Initialization Packets"); + } + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised: %s", get_name()); + + while (true) { + // Request data. If error occurs notify. + if (!request_data()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Request Data Error"); + } + + // Sleep the scheduler + hal.scheduler->delay_microseconds(1000); + + // Send Aiding data to the device + if (option_is_set(AP_ExternalAHRS::OPTIONS::ARSP_AID)) { + if (!send_airspeed_aiding()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ExternalAHRS: Unable to send airspeed aiding."); + } + } + + // Collect the requested packets from the UART manager + if (!get_packets()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Error Receiving Packets"); + } + } +} + +bool AP_ExternalAHRS_AdvancedNavigation::get_packets(void) +{ + if (_uart == nullptr) { + return false; + } + // Ensure that this section is completed in a single thread. + WITH_SEMAPHORE(_sem); + + // guard for no data on uart. + if (_uart->available() <= 0) { + return true; + } + + // receive packets from the UART into the decoder + _decoder.receive(_uart->read(_decoder.pointer(), _decoder.size())); + + if (_decoder.bytes_received() > 0) { + // Decode all packets in the buffer + while (!_decoder.is_complete()) { + // decode a packet into the message buffer + if (!_decoder.decode_packet(_msg.buffer, sizeof(_msg.buffer))) { + return false; + } + handle_packet(); + } + } + return true; +} + +bool AP_ExternalAHRS_AdvancedNavigation::request_device_information(void) +{ + /* + Packet for requesting a Advanced Navigation Device to send its + device information (ANPP Packet 3) a single time. + 0x9a-0xd1 - Header see ANPP Documentation for more info + 0x03 - Request Packet 3 (Device Info Packet) + */ + const uint8_t request_an_info[] {0x9a, 0x01, 0x01, 0x93, 0xd1, 0x03}; + return !(_uart->txspace() < sizeof(request_an_info) || _uart->write(request_an_info, sizeof(request_an_info)) != sizeof(request_an_info)); +} + +bool AP_ExternalAHRS_AdvancedNavigation::request_data(void) +{ + // Update device info every 20 secs + if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || !_last_device_info_pkt_ms) { + if (!request_device_information()) { + return false; + } + } + + // Don't send a packet request unless the device is healthy + if (_current_rate != get_rate() && healthy()) { + if (!sendPacketRequest()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Failure to send packet request"); + } + } + + // check for updates to environment that require INS filter updates + if (_fly_forward != in_fly_forward() || _gnss_disable != gnss_is_disabled()){ + _fly_forward = in_fly_forward(); + _gnss_disable = gnss_is_disabled(); + + // Select AdNav vehicle for current flight mode. + vehicle_type_e vehicle_type = vehicle_type_3d_aircraft; + if (_fly_forward == true) { + vehicle_type = vehicle_type_fixed_wing_plane; + } + + set_filter_options(!_gnss_disable, vehicle_type); + } + return true; +} + +int8_t AP_ExternalAHRS_AdvancedNavigation::get_port(void) const +{ + if (_uart == nullptr) { + return -1; + } + return _port_num; +}; + +// Get model/type name +const char* AP_ExternalAHRS_AdvancedNavigation::get_name() const +{ + if ((AP_HAL::millis() - _last_pkt_ms) > AN_TIMEOUT) { + static char buf[30]; + hal.util->snprintf(buf, 30, "AdNav: TIMEOUT... %8ums", (unsigned int) (AP_HAL::millis() - _last_pkt_ms)); + return buf; + } + + if (_last_device_info_pkt_ms == 0) { + return "AdNav No Connection..."; + } + + switch (_device_id) { + case 0: + return "Uninitialized Device ID"; + break; + case device_id_spatial: + return "AdNav Spatial"; + break; + case device_id_orientus: + case device_id_orientus_v3: + return "AdNav Orientus"; + break; + case device_id_spatial_fog: + return "AdNav Spatial FOG"; + break; + case device_id_spatial_dual: + return "AdNav Spatial Dual"; + break; + case device_id_ilu: + return "AdNav Interface Logging Unit"; + break; + case device_id_air_data_unit: + return "AdNav Air Data Unit"; + break; + case device_id_spatial_fog_dual: + return "AdNav Spatial FOG Dual"; + break; + case device_id_motus: + return "AdNav Motus"; + break; + case device_id_gnss_compass: + return "AdNav GNSS Compass"; + break; + case device_id_certus: + return "AdNav Certus"; + break; + case device_id_aries: + return "AdNav Aries"; + break; + case device_id_boreas_d90: + case device_id_boreas_d90_fpga: + case device_id_boreas_coil: + return "AdNav Boreas"; + break; + case device_id_certus_mini_a: + return "AdNav Certus Mini A"; + break; + case device_id_certus_mini_n: + return "AdNav Certus Mini N"; + break; + case device_id_certus_mini_d: + return "AdNav Certus Mini D"; + break; + default: + return "Unknown AdNav Device ID"; + } +} + +bool AP_ExternalAHRS_AdvancedNavigation::healthy(void) const +{ + uint32_t now = AP_HAL::millis(); + return ((now - _last_state_pkt_ms) < 500); +} + +bool AP_ExternalAHRS_AdvancedNavigation::initialised(void) const +{ + if (get_gnss_capability()) { + return _last_state_pkt_ms != 0 && _last_device_info_pkt_ms != 0 && _last_raw_gnss_pkt_ms !=0; + } else { + return _last_state_pkt_ms != 0 && _last_device_info_pkt_ms != 0; + } +} + +bool AP_ExternalAHRS_AdvancedNavigation::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + // Add failure messages + if (AP_HAL::millis() - _last_pkt_ms > AN_TIMEOUT) { + hal.util->snprintf(failure_msg, failure_msg_len, "DEVICE TIMEOUT Last Packet %8ums ago", (unsigned int) (AP_HAL::millis() - _last_pkt_ms)); + return false; + } + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "Device unhealthy"); + return false; + } + if (_device_status.system.b.gnss_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "GNSS Failure"); + return false; + } + if (_device_status.system.b.system_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "System Failure"); + return false; + } + if (_device_status.system.b.accelerometer_sensor_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "Accelerometer Failure"); + return false; + } + if (_device_status.system.b.gyroscope_sensor_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "Gyroscope Failure"); + return false; + } + if (_device_status.system.b.magnetometer_sensor_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "Magnetometer Failure"); + return false; + } + if (_device_status.system.b.pressure_sensor_failure) { + hal.util->snprintf(failure_msg, failure_msg_len, "Barometer Failure"); + return false; + } + if ((_device_status.filter.b.gnss_fix_type < 1) && (get_gnss_capability())) { + hal.util->snprintf(failure_msg, failure_msg_len, "No GPS lock"); + return false; + } + if (!_device_status.filter.b.orientation_filter_initialised) { + hal.util->snprintf(failure_msg, failure_msg_len, "Orientation Filter Not Initialised"); + return false; + } + if (!_device_status.filter.b.ins_filter_initialised) { + hal.util->snprintf(failure_msg, failure_msg_len, "INS Filter Not Initialised"); + return false; + } + if (!_device_status.filter.b.heading_initialised) { + hal.util->snprintf(failure_msg, failure_msg_len, "Heading Filter Not Initialised"); + return false; + } + return true; +} + +void AP_ExternalAHRS_AdvancedNavigation::get_filter_status(nav_filter_status &status) const +{ + memset(&status, 0, sizeof(status)); + + if (_last_state_pkt_ms == 0) { + return; + } + status.flags.initalized = true; + if (!healthy()) { + return; + } + + status.flags.vert_pos = true; + status.flags.attitude = true; + status.flags.vert_vel = true; + + if (_device_status.filter.b.gnss_fix_type > gnss_fix_none) { + status.flags.horiz_vel = true; + status.flags.horiz_pos_rel = true; + status.flags.horiz_pos_abs = true; + status.flags.pred_horiz_pos_rel = true; + status.flags.pred_horiz_pos_abs = true; + status.flags.using_gps = true; + } + + if (_device_status.filter.b.gnss_fix_type > gnss_fix_2d) { + status.flags.gps_quality_good = true; + } + +} + +void AP_ExternalAHRS_AdvancedNavigation::send_status_report(GCS_MAVLINK &link) const +{ + // prepare flags + uint16_t flags = 0; + nav_filter_status filterStatus; + get_filter_status(filterStatus); + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (!filterStatus.flags.initalized) { + flags |= EKF_UNINITIALIZED; + } + + // send message + const float vel_gate = 5; // represents hz value data is posted at + const float pos_gate = 5; // represents hz value data is posted at + const float hgt_gate = 5; // represents hz value data is posted at + const float mag_var = 5; + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + norm(_last_vel_sd->sd[0], + _last_vel_sd->sd[1], + _last_vel_sd->sd[2])/vel_gate, + norm(_gnss_sd.x, + _gnss_sd.y)/pos_gate, + _gnss_sd.z/hgt_gate, + mag_var, + 0, + 0); +} + +/* + * Function to request data packets from a Advanced Navigation Device at the current rate. + */ +bool AP_ExternalAHRS_AdvancedNavigation::sendPacketRequest() +{ + /* + Packet for requesting a Advanced Navigation Device to set its + Packet Timer Period to 1000Hz + 0xa0-0x6c - Header see ANPP Documentation for more info + 0x01 - True - Permanent effect + 0x01 - True - UTC Sync + 0xe8,0x03 - Packet Timer Period ms (uint16_t) (1000Hz) + */ + const uint8_t timer_period_1000_hz[] {0xa0, 0xb4, 0x04, 0x3c, 0x6c, 0x01, 0x01, 0xe8, 0x03}; + + if (_uart->txspace() < sizeof(timer_period_1000_hz)) { + return false; + } + _uart->write(timer_period_1000_hz, sizeof(timer_period_1000_hz)); + + // Update the current rate + _current_rate = get_rate(); + + const AN_PACKET_PERIODS periods{ + permanent: true, + clear_existing_packet_periods: true, + periods: { + AN_PERIOD{ + id: packet_id_system_state, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: packet_id_velocity_standard_deviation, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: packet_id_raw_sensors, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: packet_id_raw_gnss, + packet_period: (uint32_t) 1.0e3 / AN_GNSS_PACKET_RATE + }, + AN_PERIOD{ + id: packet_id_satellites, + packet_period: (uint32_t) 1.0e3 / _current_rate + } + } + }; + + AN_PACKET packet; + + // load the AN_PACKETS_PERIOD Into the payload. + packet.payload.packet_periods = periods; + packet.update_checks(packet_id_packet_periods, packet.getPeriodsLength(periods)); + + // Check for space in the tx buffer + if (_uart->txspace() < packet.packet_size()) { + return false; + } + _uart->write(packet.raw_pointer(), packet.packet_size()); + + return true; +} + +/* + * Function that returns the gps capability of the connected AdNav device. + */ +bool AP_ExternalAHRS_AdvancedNavigation::get_gnss_capability(void) const +{ + switch (_device_id) { + case device_id_orientus: + case device_id_orientus_v3: + case device_id_air_data_unit: + case device_id_motus: + case device_id_certus_mini_a: + return false; + default: + return true; + } +} + +/* + * Function that returns the barometric capability of the connected AdNav device. + */ +bool AP_ExternalAHRS_AdvancedNavigation::get_baro_capability(void) const +{ + switch (_device_id) { + case device_id_air_data_unit: + case device_id_orientus: + case device_id_orientus_v3: + case device_id_gnss_compass: + case device_id_certus_mini_a: + return false; + case device_id_motus: + // Motus versions prior to 2.3 didn't have a barometer enabled. + if (_hardware_rev < 2300) { + return false; + } + break; + default: + break; + } + return true; +} + +bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent) +{ + + AN_FILTER_OPTIONS options_packet; + + options_packet.permanent = permanent; + options_packet.vehicle_type = vehicle_type; + options_packet.internal_gnss_enabled = gnss_en; + options_packet.magnetometers_enabled = false; + options_packet.atmospheric_altitude_enabled = true; + options_packet.velocity_heading_enabled = false; + options_packet.reversing_detection_enabled = false; + options_packet.motion_analysis_enabled = false; + options_packet.automatic_magnetic_calibration_enabled = true; + options_packet.dual_antenna_disabled = false; + // set reserved packets to 0 + memset(options_packet.reserved, 0, sizeof(options_packet.reserved)); + + return set_filter_options(options_packet); +} + +bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(const AN_FILTER_OPTIONS options_packet) +{ + AN_PACKET packet; + + packet.payload.filter_options = options_packet; + packet.update_checks(packet_id_filter_options, sizeof(packet.payload.filter_options)); + + // Check for space in the tx buffer + if (_uart->txspace() < packet.packet_size()) { + return false; + } + _uart->write(packet.raw_pointer(), packet.packet_size()); + + return true; +} + +// Returns true on successful transmit of packet or if called prior to requirement of sending packet. +bool AP_ExternalAHRS_AdvancedNavigation::send_airspeed_aiding(){ + + uint32_t now = AP_HAL::millis(); + const AP_DAL_Airspeed *arsp = dal.airspeed(); + const AP_DAL_Baro &bar = dal.baro(); + uint32_t period_airspeed_aiding_ms = 1000/AN_AIRSPEED_AIDING_FREQUENCY; + if (now < _last_ext_air_data_sent_ms + period_airspeed_aiding_ms) { + return true; + } + + if (arsp == nullptr || !arsp->healthy() || !bar.healthy(bar.get_primary())) { + return false; + } + + AN_PACKET packet; + float airspeed = arsp->get_airspeed(); + + _last_ext_air_data_sent_ms = now; + + packet.payload.ext_air_data.true_airspeed = airspeed * dal.get_EAS2TAS(); + packet.payload.ext_air_data.airspeed_delay = (AP_HAL::millis() - arsp->last_update_ms()) / 1000; + packet.payload.ext_air_data.airspeed_standard_deviation = get_airspeed_error(airspeed); + packet.payload.ext_air_data.barometric_altitude = bar.get_altitude(); + packet.payload.ext_air_data.baro_delay = (AP_HAL::millis() - bar.get_last_update()) / 1000; + packet.payload.ext_air_data.barometric_standard_deviation = get_pressure_error(); + packet.payload.ext_air_data.flags.b.airspeed_set_valid = true; + packet.payload.ext_air_data.flags.b.barometric_altitude_set_valid = true; + packet.update_checks(packet_id_external_air_data, sizeof(packet.payload.ext_air_data)); + + // Check for space in the tx buffer + if (_uart->txspace() < packet.packet_size()) { + return false; + } + _uart->write(packet.raw_pointer(), packet.packet_size()); + + return true; +} + +// Method to return estimate of airspeed error based upon the current airspeed given error at 20m/s. +float AP_ExternalAHRS_AdvancedNavigation::get_airspeed_error(float airspeed) +{ + float nominal_pressure = 0.5 * sq(airspeed); + float adj_airspeed = sqrt(2 * nominal_pressure + get_pressure_error()); + return adj_airspeed - airspeed; +} + +// Method to give an estimate of the pressure error from the parameter for airspeed error at 20m/s +float AP_ExternalAHRS_AdvancedNavigation::get_pressure_error(void) +{ + return (0.5*(sq(20+airspeed_err_20ms()))) - (0.5*sq(20)); +} + +void AP_ExternalAHRS_AdvancedNavigation::send_ack_text(const char* packet_name, uint8_t result) +{ + if (result) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS: Acknowledgement of %s Failure: Code %d", packet_name, result); + return; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS: Acknowledgement of %s success", packet_name); +} + +void AP_ExternalAHRS_AdvancedNavigation::handle_packet() +{ + // get current time + uint32_t now = AP_HAL::millis(); + _last_pkt_ms = now; + + // Update depending on received packet. + switch (_msg.packet.id) { + case packet_id_acknowledge: { + // const char* packet_prefix; + switch (_msg.packet.payload.acknowledge.id_acknowledged){ + case packet_id_acknowledge: + return; + case packet_id_filter_options: + send_ack_text("Filter Options", _msg.packet.payload.acknowledge.result); + return; + case packet_id_packet_periods: + send_ack_text("Packet Periods", _msg.packet.payload.acknowledge.result); + return; + case packet_id_packet_timer_period: + send_ack_text("Packet Timer Period", _msg.packet.payload.acknowledge.result); + return; + default: + char buffer [15]; + snprintf(buffer, sizeof(buffer), "Packet %d", _msg.packet.payload.acknowledge.id_acknowledged); + send_ack_text(buffer, _msg.packet.payload.acknowledge.result); + return; + } + break; + } + case packet_id_device_information: { + _last_device_info_pkt_ms = now; + _device_id = _msg.packet.payload.device_info.device_id; + _hardware_rev = _msg.packet.payload.device_info.hardware_revision; + break; + } + case packet_id_system_state: { + _last_state_pkt_ms = now; + // Save the status + _device_status.system.r = _msg.packet.payload.system_state.status.system.r; + _device_status.filter.r = _msg.packet.payload.system_state.status.filter.r; + { + WITH_SEMAPHORE(state.sem); + + state.accel = Vector3f{ + _msg.packet.payload.system_state.body_acceleration[0], + _msg.packet.payload.system_state.body_acceleration[1], + _msg.packet.payload.system_state.body_acceleration[2] + }; + + state.gyro = Vector3f{ + _msg.packet.payload.system_state.angular_velocity[0], + _msg.packet.payload.system_state.angular_velocity[1], + _msg.packet.payload.system_state.angular_velocity[2] + }; + + state.have_velocity = true; + state.velocity = Vector3f{ + _msg.packet.payload.system_state.velocity_ned[0], + _msg.packet.payload.system_state.velocity_ned[1], + _msg.packet.payload.system_state.velocity_ned[2] + }; + + if (get_gnss_capability()) { + state.have_location = true; + const double latlon_scale = degrees(1) * 1.0e7; + state.location = Location{ + (int32_t) (_msg.packet.payload.system_state.llh[0] * latlon_scale), + (int32_t) (_msg.packet.payload.system_state.llh[1] * latlon_scale), + (int32_t) (_msg.packet.payload.system_state.llh[2] *1.0e2), + Location::AltFrame::ABSOLUTE + }; + if (!state.have_origin) { + state.origin = state.location; + state.have_origin = true; + } + state.last_location_update_us = AP_HAL::micros(); + } + + state.have_quaternion = true; + state.quat.from_euler( + _msg.packet.payload.system_state.rph[0], + _msg.packet.payload.system_state.rph[1], + _msg.packet.payload.system_state.rph[2] + ); + } + break; + } + case packet_id_velocity_standard_deviation: { + // save packet to be used for external gps. + *_last_vel_sd = _msg.packet.payload.velocity_standard_deviation; + break; + } + + case packet_id_raw_sensors: { + AP_ExternalAHRS::ins_data_message_t ins{ + accel: Vector3f{ + _msg.packet.payload.raw_sensors.accelerometers[0], + _msg.packet.payload.raw_sensors.accelerometers[1], + _msg.packet.payload.raw_sensors.accelerometers[2] + }, + + gyro: Vector3f{ + _msg.packet.payload.raw_sensors.gyroscopes[0], + _msg.packet.payload.raw_sensors.gyroscopes[1], + _msg.packet.payload.raw_sensors.gyroscopes[2] + }, + temperature: _msg.packet.payload.raw_sensors.imu_temperature + }; + AP::ins().handle_external(ins); + + +#if AP_COMPASS_EXTERNALAHRS_ENABLED + AP_ExternalAHRS::mag_data_message_t mag { + field: Vector3f{ + _msg.packet.payload.raw_sensors.magnetometers[0], + _msg.packet.payload.raw_sensors.magnetometers[1], + _msg.packet.payload.raw_sensors.magnetometers[2] + }, + }; + AP::compass().handle_external(mag); +#endif +#if AP_BARO_EXTERNALAHRS_ENABLED + if (get_baro_capability()) { + AP_ExternalAHRS::baro_data_message_t baro{ + instance: 0, + pressure_pa: _msg.packet.payload.raw_sensors.pressure, + temperature: _msg.packet.payload.raw_sensors.pressure_temperature + }; + AP::baro().handle_external(baro); + }; +#endif + } + break; + + case packet_id_raw_gnss: { + // Save the standard deviations for status report + _gnss_sd = Vector3f{ + _msg.packet.payload.raw_gnss.llh_standard_deviation[0], + _msg.packet.payload.raw_gnss.llh_standard_deviation[1], + _msg.packet.payload.raw_gnss.llh_standard_deviation[2] + }; + + AP_ExternalAHRS::gps_data_message_t gps; + + const uint32_t unix_sec = _msg.packet.payload.raw_gnss.unix_time; + const uint32_t unix_usec = _msg.packet.payload.raw_gnss.unix_microseconds; + uint8_t fix = 0; + + switch (_msg.packet.payload.raw_gnss.flags.b.fix_type) { + case gnss_fix_none: + fix = GPS_FIX_TYPE_NO_FIX; + break; + case gnss_fix_2d: + fix = GPS_FIX_TYPE_2D_FIX; + break; + case gnss_fix_3d: + fix = GPS_FIX_TYPE_3D_FIX; + break; + case gnss_fix_sbas: + case gnss_fix_differential: + fix = GPS_FIX_TYPE_DGPS; + break; + case gnss_fix_omnistar: + fix = GPS_FIX_TYPE_PPP; + break; + case gnss_fix_rtk_float: + fix = GPS_FIX_TYPE_RTK_FLOAT; + break; + case gnss_fix_rtk_fixed: + fix = GPS_FIX_TYPE_RTK_FIXED; + break; + default: + break; + } + + const uint32_t leapseconds = 18U; + const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - leapseconds; + const uint32_t epoch_seconds = unix_sec - epoch; + gps.gps_week = epoch_seconds / AP_SEC_PER_WEEK; + const uint32_t t_ms = unix_usec / 1000U; + // round time to nearest 200ms + gps.ms_tow = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200); + + gps.fix_type = fix; + gps.satellites_in_view = (uint8_t) (_last_satellites->beidou_satellites + _last_satellites->galileo_satellites + + _last_satellites->glonass_satellites + _last_satellites->gps_satellites + _last_satellites->sbas_satellites); + + gps.horizontal_pos_accuracy = (float) norm(_msg.packet.payload.raw_gnss.llh_standard_deviation[0], _msg.packet.payload.raw_gnss.llh_standard_deviation[1]); + gps.vertical_pos_accuracy = _msg.packet.payload.raw_gnss.llh_standard_deviation[2]; + + gps.hdop = _last_satellites->hdop; + gps.vdop = _last_satellites->vdop; + + gps.latitude = (int32_t) (degrees(_msg.packet.payload.raw_gnss.llh[0]) * 1.0e7); + gps.longitude = (int32_t) (degrees(_msg.packet.payload.raw_gnss.llh[1]) * 1.0e7); + gps.msl_altitude = (int32_t) (_msg.packet.payload.raw_gnss.llh[2] * 1.0e2); + + gps.ned_vel_north = _msg.packet.payload.raw_gnss.velocity_ned[0]; + gps.ned_vel_east = _msg.packet.payload.raw_gnss.velocity_ned[1]; + gps.ned_vel_down = _msg.packet.payload.raw_gnss.velocity_ned[2]; + + gps.horizontal_vel_accuracy = (float) norm( + _last_vel_sd->sd[0], + _last_vel_sd->sd[1], + _last_vel_sd->sd[2] + ); + + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps, instance); + } + } + break; + + case packet_id_satellites: { + // save packet to be used for external gps. + *_last_satellites = _msg.packet.payload.satellites; + } + break; + + default: { + + } + break; + } +} + +#endif // AP_EXTERNAL_AHRS_ADNAV_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h new file mode 100644 index 0000000000000..0a647c3b59f42 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -0,0 +1,548 @@ +/* + 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 . + */ +/* + For more information on ANPP and Advanced Navigation Devices please + see the following: + Website: https://www.advancednavigation.com/ + ANPP: https://docs.advancednavigation.com/certus/ANPP/Advanced%20Navigation%20Packet.htm + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + +#include "AP_ExternalAHRS_backend.h" +#include +#include +#include + +#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_START_SYSTEM_PACKETS 0 +#define AN_START_STATE_PACKETS 20 +#define AN_START_CONFIGURATION_PACKETS 180 + + +class AP_ExternalAHRS_AdvancedNavigation_Decoder +{ +public: + 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; + size_t _bytes_received; + + int decode_packet(uint8_t* out_buffer, size_t buf_size); + + uint8_t* pointer() + { + return &_buffer[_buffer_length]; + } + + size_t size() const + { + return sizeof(_buffer) - _buffer_length; + } + + void receive(size_t received) + { + _complete = false; + _bytes_received = received; + _buffer_length += received; + } + + size_t bytes_received() const + { + return _bytes_received; + } + + bool is_complete() const + { + return _complete; + } + + uint8_t calculate_header_lrc(uint8_t* data) const + { + return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1; + }; +private: + bool _complete = false; +}; + +class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend +{ +public: + + AP_ExternalAHRS_AdvancedNavigation(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + + // get serial port number, -1 for not enabled + int8_t get_port(void) const override; + + // Get model/type name + const char* get_name() const override; + + // accessors for AP_AHRS + bool healthy(void) const override; + bool initialised(void) const override; + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; + void get_filter_status(nav_filter_status &status) const override; + void send_status_report(class GCS_MAVLINK &link) const override; + + // check for new data + void update() override; + +protected: + uint8_t num_gps_sensors(void) const override + { + return 1; + } + +private: + AP_ExternalAHRS_AdvancedNavigation_Decoder _decoder; + AP_DAL &dal; + + typedef enum { + packet_id_acknowledge, + packet_id_request, + packet_id_boot_mode, + packet_id_device_information, + packet_id_restore_factory_settings, + packet_id_reset, + packet_id_6_reserved, + packet_id_file_transfer_request, + packet_id_file_transfer_acknowledge, + packet_id_file_transfer, + packet_id_serial_port_passthrough, + packet_id_ip_configuration, + packet_id_12_reserved, + packet_id_extended_device_information, + packet_id_subcomponent_information, + end_system_packets, + + packet_id_system_state = AN_START_STATE_PACKETS, + packet_id_unix_time, + packet_id_formatted_time, + packet_id_status, + packet_id_position_standard_deviation, + packet_id_velocity_standard_deviation, + packet_id_euler_orientation_standard_deviation, + packet_id_quaternion_orientation_standard_deviation, + packet_id_raw_sensors, + packet_id_raw_gnss, + packet_id_satellites, + packet_id_satellites_detailed, + packet_id_geodetic_position, + packet_id_ecef_position, + packet_id_utm_position, + packet_id_ned_velocity, + packet_id_body_velocity, + packet_id_acceleration, + packet_id_body_acceleration, + packet_id_euler_orientation, + packet_id_quaternion_orientation, + packet_id_dcm_orientation, + packet_id_angular_velocity, + packet_id_angular_acceleration, + packet_id_external_position_velocity, + packet_id_external_position, + packet_id_external_velocity, + packet_id_external_body_velocity, + packet_id_external_heading, + packet_id_running_time, + packet_id_local_magnetics, + packet_id_odometer_state, + packet_id_external_time, + packet_id_external_depth, + packet_id_geoid_height, + packet_id_rtcm_corrections, + packet_id_56_reserved, + packet_id_wind, + packet_id_heave, + packet_id_59_reserved, + packet_id_raw_satellite_data, + packet_id_raw_satellite_ephemeris, + packet_id_62_reserved, + packet_id_63_reserved, + packet_id_64_reserved, + packet_id_65_reserved, + packet_id_gnss_summary, + packet_id_external_odometer, + packet_id_external_air_data, + packet_id_gnss_receiver_information, + packet_id_raw_dvl_data, + packet_id_north_seeking_status, + packet_id_gimbal_state, + packet_id_automotive, + packet_id_74_reserved, + packet_id_external_magnetometers, + packet_id_76_reserved, + packet_id_77_reserved, + packet_id_78_reserved, + packet_id_79_reserved, + packet_id_basestation, + packet_id_81_reserved, + packet_id_82_reserved, + packet_id_zero_angular_velocity, + packet_id_extended_satellites, + packet_id_sensor_temperatures, + packet_id_system_temperature, + packet_id_87_reserved, + end_state_packets, + + packet_id_packet_timer_period = AN_START_CONFIGURATION_PACKETS, + packet_id_packet_periods, + packet_id_baud_rates, + packet_id_183_reserved, + packet_id_sensor_ranges, + packet_id_installation_alignment, + packet_id_filter_options, + packet_id_187_reserved, + packet_id_gpio_configuration, + packet_id_magnetic_calibration_values, + packet_id_magnetic_calibration_configuration, + packet_id_magnetic_calibration_status, + packet_id_odometer_configuration, + packet_id_zero_alignment, + packet_id_reference_offsets, + packet_id_gpio_output_configuration, + packet_id_dual_antenna_configuration, + packet_id_gnss_configuration, + packet_id_user_data, + packet_id_gpio_input_configuration, + packet_id_200_reserved, + packet_id_201_reserved, + packet_id_ip_dataports_configuration, + packet_id_can_configuration, + packet_id_device_name, + end_configuration_packets + } packet_id_e; + + typedef enum { + gnss_fix_none, + gnss_fix_2d, + gnss_fix_3d, + gnss_fix_sbas, + gnss_fix_differential, + gnss_fix_omnistar, + gnss_fix_rtk_float, + gnss_fix_rtk_fixed + } gnss_fix_type_e; + + typedef enum { + device_id_spatial = 1, + device_id_orientus = 3, + device_id_spatial_fog, + device_id_spatial_dual, + device_id_obdii_odometer = 10, + device_id_orientus_v3, + device_id_ilu, + device_id_air_data_unit, + device_id_spatial_fog_dual = 16, + device_id_motus, + device_id_gnss_compass = 19, + device_id_certus = 26, + device_id_aries, + device_id_boreas_d90, + device_id_boreas_d90_fpga = 35, + device_id_boreas_coil, + device_id_certus_mini_a = 49, + device_id_certus_mini_n, + device_id_certus_mini_d, + } device_id_e; + + typedef enum { + vehicle_type_unlimited, + vehicle_type_bicycle, + vehicle_type_car, + vehicle_type_hovercraft, + vehicle_type_submarine, + vehicle_type_3d_underwater, + vehicle_type_fixed_wing_plane, + vehicle_type_3d_aircraft, + vehicle_type_human, + vehicle_type_small_boat, + vehicle_type_ship, + vehicle_type_stationary, + vehicle_type_stunt_plane, + vehicle_type_race_car + } vehicle_type_e; + + struct PACKED AN_ACKNOWLEDGE { + uint8_t id_acknowledged; + uint16_t crc_acknowledged; + uint8_t result; + }; + + struct PACKED AN_DEVICE_INFO { + uint32_t software_version; + uint32_t device_id; + uint32_t hardware_revision; + uint32_t serial_1; + uint32_t serial_2; + uint32_t serial_3; + }; + + struct PACKED AN_STATUS{ + 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; + 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; + }; + + struct PACKED AN_SYSTEM_STATE { + AN_STATUS status; + uint32_t unix_time_seconds; + uint32_t microseconds; + double llh[3]; //rad,rad,m + float velocity_ned[3]; // m/s + float body_acceleration[3]; // m/s/s + float g_force; // g's + float rph[3]; // rad + float angular_velocity[3]; // rad/s + float llh_standard_deviation[3]; //m + }; + + struct PACKED AN_VELOCITY_STANDARD_DEVIATION { + float sd[3]; + } *_last_vel_sd; + + struct PACKED AN_RAW_SENSORS { + public: + float accelerometers[3]; // m/s/s + float gyroscopes[3]; // rad/s + float magnetometers[3]; // mG + float imu_temperature; // deg C + float pressure; //Pascals + float pressure_temperature; // deg C + }; + + struct PACKED AN_RAW_GNSS { + uint32_t unix_time; + uint32_t unix_microseconds; + double llh[3]; //rad,rad,m + float velocity_ned[3]; // m/s + float llh_standard_deviation[3]; //m + float tilt; // rad + float heading; + float tilt_sd; + float heading_sd; + 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; + }; + + struct PACKED AN_SATELLITES { + float hdop; + float vdop; + uint8_t gps_satellites; + uint8_t glonass_satellites; + uint8_t beidou_satellites; + uint8_t galileo_satellites; + uint8_t sbas_satellites; + } *_last_satellites; + + struct PACKED AN_EXTERNAL_AIR_DATA { + float baro_delay; + float airspeed_delay; + float barometric_altitude; + float true_airspeed; + float barometric_standard_deviation; + float airspeed_standard_deviation; + union { + uint8_t r; + struct{ + uint8_t barometric_altitude_set_valid :1; + uint8_t airspeed_set_valid :1; + uint8_t barometric_altitude_reference_reset :1; + }b; + } flags; + }; + + struct PACKED AN_PERIOD { + uint8_t id; + uint32_t packet_period; + }; + + struct PACKED AN_PACKET_PERIODS { + uint8_t permanent; + uint8_t clear_existing_packet_periods; + AN_PERIOD periods[(AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD)]; + }; + + struct PACKED AN_FILTER_OPTIONS { + uint8_t permanent; + uint8_t vehicle_type; + uint8_t internal_gnss_enabled; + uint8_t magnetometers_enabled; + uint8_t atmospheric_altitude_enabled; + uint8_t velocity_heading_enabled; + uint8_t reversing_detection_enabled; + uint8_t motion_analysis_enabled; + uint8_t automatic_magnetic_calibration_enabled; + uint8_t dual_antenna_disabled; + uint8_t reserved[7]; + }; + + class PACKED AN_PACKET + { + public: + uint8_t lrc; + uint8_t id; + uint8_t length; + uint16_t crc; + + union payload { + uint8_t raw_packet[AN_MAXIMUM_PACKET_SIZE]; + AN_ACKNOWLEDGE acknowledge; + AN_DEVICE_INFO device_info; + AN_SYSTEM_STATE system_state; + AN_VELOCITY_STANDARD_DEVIATION velocity_standard_deviation; + AN_RAW_SENSORS raw_sensors; + AN_RAW_GNSS raw_gnss; + AN_SATELLITES satellites; + AN_EXTERNAL_AIR_DATA ext_air_data; + AN_PACKET_PERIODS packet_periods; + AN_FILTER_OPTIONS filter_options; + } payload; + + void update_checks(uint8_t header_id, uint8_t payload_length) + { + length = payload_length; + + // Update the packet check and header id + crc = crc16_ccitt(payload.raw_packet, payload_length, 0xFFFF); + id = header_id; + + // Update the header LRC + uint8_t* id_ptr = &id; + lrc = ((id_ptr[0] + id_ptr[1] + id_ptr[2] + id_ptr[3]) ^ 0xFF) + 1; + } + + uint8_t getPeriodsLength(AN_PACKET_PERIODS packet_periods) + { + // Find the utilized size of the packet and make it the payload length. + for (uint32_t idx = 0; idx < (AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD); idx++) + { + if (packet_periods.periods[idx].id == 0 && packet_periods.periods[idx].packet_period == 0) { + return 2 + (sizeof(AN_PERIOD)*idx); + } + } + return (AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD); + } + + uint8_t* raw_pointer() + { + return &lrc; + } + + size_t packet_size() const + { + return AN_PACKET_HEADER_SIZE + length; + } + }; + + union PACKED MsgUnion { + MsgUnion() { } + uint8_t buffer[AN_PACKET_HEADER_SIZE + AN_MAXIMUM_PACKET_SIZE]; + AN_PACKET packet; + } _msg; + + uint16_t _current_rate; + AN_STATUS _device_status; + Vector3f _gnss_sd; + + AP_HAL::UARTDriver *_uart; + HAL_Semaphore _sem; + + uint32_t _baudrate; + int8_t _port_num; + + uint32_t _last_pkt_ms; + uint32_t _last_state_pkt_ms; + uint32_t _last_device_info_pkt_ms; + uint32_t _last_raw_gnss_pkt_ms; + uint32_t _last_ext_air_data_sent_ms; + uint32_t _device_id; + uint32_t _hardware_rev; + + bool _gnss_disable; + bool _fly_forward; + + void update_thread(); + bool get_packets(void); + bool request_device_information(void); + bool request_data(void); + bool sendPacketRequest(void); + bool get_gnss_capability(void) const; + bool get_baro_capability(void) const; + bool set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent = false); + bool set_filter_options(AN_FILTER_OPTIONS options_packet); + bool send_airspeed_aiding(void); + float get_airspeed_error(float airspeed); + float get_pressure_error(void); + void send_ack_text(const char* packet_name, uint8_t result); + void handle_packet(); +}; + +#endif // AP_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index e5a22a87cbe44..1ac71dee4f931 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -61,6 +61,13 @@ class AP_ExternalAHRS_backend { frontend.set_default_sensors(sensors); } + /* + return the airspeed error at 20ms + */ + float airspeed_err_20ms(void) const { + return frontend.arsp_err_20ms; + } + /* return true if the GNSS is disabled */ diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 650f605b0a9f2..32d9d3c83ebbc 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -29,3 +29,7 @@ #ifndef AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED #define AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_EXTERNAL_AHRS_ADNAV_ENABLED +#define AP_EXTERNAL_AHRS_ADNAV_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 54a530bc447b1..eb4827b9fcf6d 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -295,6 +295,12 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const inertiallabs = NEW_NOTHROW SITL::InertialLabs(); return inertiallabs; + } else if (streq(name, "AdNav")) { + if (adnav != nullptr) { + AP_HAL::panic("Only one AdNav at a time"); + } + adnav = new SITL::AdNav(); + return adnav; #if HAL_SIM_AIS_ENABLED } else if (streq(name, "AIS")) { if (ais != nullptr) { @@ -479,6 +485,10 @@ void SITL_State_Common::sim_update(void) inertiallabs->update(); } + if (adnav != nullptr) { + adnav->update(); + } + #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { ais->update(); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 95ef129c1cce7..9ccf23a4c191b 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -208,6 +209,9 @@ class HALSITL::SITL_State_Common { // simulated InertialLabs INS SITL::InertialLabs *inertiallabs; + + // simulated AdNav system: + SITL::AdNav *adnav; #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend diff --git a/libraries/SITL/SIM_AdNav.cpp b/libraries/SITL/SIM_AdNav.cpp new file mode 100644 index 0000000000000..26fd945ecb203 --- /dev/null +++ b/libraries/SITL/SIM_AdNav.cpp @@ -0,0 +1,517 @@ +/* + 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 + } + + if (now - _last_gnss_sent_us >= _gnss_period_us) { + _last_gnss_sent_us = now; + 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 %u\n", unsigned(an_packet->id)); + 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.yawDeg); + 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 pressure, temp_k; + AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude+rand_float()*0.25, pressure, temp_k); + raw_sensors.pressure = pressure; + raw_sensors.pressure_temperature = KELVIN_TO_C(temp_k); + + 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]); +} diff --git a/libraries/SITL/SIM_AdNav.h b/libraries/SITL/SIM_AdNav.h new file mode 100644 index 0000000000000..255cfeb98b9c9 --- /dev/null +++ b/libraries/SITL/SIM_AdNav.h @@ -0,0 +1,272 @@ +/* + 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 _gnss_period_us = 200000; // Period to send packets. + + uint32_t _last_pkt_sent_us; + uint32_t _last_gnss_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); + + +}; + +} +