From 373cd938a4bcbcdfbbe148f93a8c566f49dc5208 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 Sep 2023 17:19:50 +1000 Subject: [PATCH] AP_ExternalAHRS: Added Advanced Navigation Device Support EAHRS_TYPE: 3 (AdNav) has been added to the AP_External_AHRS.h/.cpp files. New Class AP_ExternalAHRS_AdvancedNavigation added in new .cpp/.h files. Supports any Advanced Navigation Packet Protocol (ANPP) device with different devices having slight behaviour changes. --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 6 + libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 4 +- .../AP_ExternalAHRS_AdvancedNavigation.cpp | 786 ++++++++++++++++++ .../AP_ExternalAHRS_AdvancedNavigation.h | 345 ++++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_config.h | 5 + 5 files changed, 1145 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 5a595055a4c84..5335996302e37 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -24,6 +24,7 @@ #include "AP_ExternalAHRS_backend.h" #include "AP_ExternalAHRS_VectorNav.h" #include "AP_ExternalAHRS_MicroStrain5.h" +#include "AP_ExternalAHRS_AdvancedNavigation.h" #include @@ -102,6 +103,11 @@ void AP_ExternalAHRS::init(void) case DevType::MicroStrain5: backend = new AP_ExternalAHRS_MicroStrain5(this, state); return; +#endif +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + case DevType::AdNav: + backend = new AP_ExternalAHRS_AdvancedNavigation(this, state); + break; #endif } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 1c20985fb2bb0..7bdea4fa89ae7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -49,7 +49,9 @@ class AP_ExternalAHRS { #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED MicroStrain5 = 2, #endif - // 3 reserved for AdNav +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + AdNav = 3, +#endif // 4 reserved for CINS // 5 reserved for InertialLabs // 6 reserved for Trimble diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp new file mode 100644 index 0000000000000..aa534ba3fb6a9 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -0,0 +1,786 @@ +/* + 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_AdvancedNavigation.h" + +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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_ACKNOWLEDGE 0 +#define AN_TIMEOUT 5000 //ms +#define AN_MAXIMUM_PACKET_PERIODS 50 +#define AN_GNSS_PACKET_RATE 5 + +#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]) + +extern const AP_HAL::HAL &hal; + +/* + 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) +*/ +static const uint8_t request_an_info[] {0x9a, 0x01, 0x01, 0x93, 0xd1, 0x03}; + +/* + 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) +*/ +static const uint8_t timer_period_1000_hz[] {0xa0, 0xb4, 0x04, 0x3c, 0x6c, 0x01, 0x01, 0xe8, 0x03}; + + +/* + * 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; + uint8_t header_lrc, length; + uint16_t crc; + + // Iterate through buffer until no more headers could be in buffer + while (decode_iterator + AN_PACKET_HEADER_SIZE <= _buffer_length) { + 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) + length = _buffer[decode_iterator++]; + crc = _buffer[decode_iterator++]; + crc |= _buffer[decode_iterator++] << 8; + + // 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) +{ + 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"); + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised"); +} + +void AP_ExternalAHRS_AdvancedNavigation::update() +{ + get_packets(); +} + +void AP_ExternalAHRS_AdvancedNavigation::update_thread(void) +{ + _uart->begin(_baudrate); + + 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); + + // 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_data(void) +{ + + // Update device info every 20 secs + if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || (_last_device_info_pkt_ms == 0)) { + if (_uart->txspace() < sizeof(request_an_info)) { + return false; + } + _uart->write(request_an_info, sizeof(request_an_info)); + } + + // 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"); + } + } + + return true; +} + +int8_t AP_ExternalAHRS_AdvancedNavigation::get_port(void) const +{ + if (!_uart) { + 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; + + 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; + status.flags.vert_pos = true; + status.flags.vert_vel = 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() +{ + // Set the device to use a period timer of 1000Hz + // See ANPP Packet Rates for more info + 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_PACKETS_PERIOD periods{ + permanent: true, + clear_existing_packet_periods: true, + periods: { + AN_PERIOD{ + id: AN_PACKET_ID_SYSTEM_STATE, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: AN_PACKET_ID_RAW_SENSORS, + packet_period: (uint32_t) 1.0e3 / _current_rate + }, + AN_PERIOD{ + id: AN_PACKET_ID_RAW_GNSS, + packet_period: (uint32_t) 1.0e3 / AN_GNSS_PACKET_RATE + }, + AN_PERIOD{ + id: AN_PACKET_ID_SATELLITES, + packet_period: (uint32_t) 1.0e3 / _current_rate + } + } + }; + + AN_PACKET packet; + // load the AN_PACKETS_PERIOD Into the payload. + packet.payload.packets_period = periods; + packet.update_checks(AN_PACKET_ID_PACKET_PERIODS, sizeof(packet.payload.packets_period)); + + // 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; +} + +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 AN_PACKET_ID_DEVICE_INFO: { + _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 AN_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; + state.location = Location{ + (int32_t) (degrees(_msg.packet.payload.system_state.llh[0]) * 1.0e7), + (int32_t) (degrees(_msg.packet.payload.system_state.llh[1]) * 1.0e7), + (int32_t) (_msg.packet.payload.system_state.llh[2] *1.0e2), + Location::AltFrame::ABSOLUTE + }; + 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 AN_PACKET_ID_VELOCITY_STANDARD_DEVIATION: { + // save packet to be used for external gps. + *_last_vel_sd = _msg.packet.payload.velocity_standard_deviation; + break; + } + + case AN_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 AN_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 AN_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..4a6a4b540b6f0 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -0,0 +1,345 @@ +/* + 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_backend.h" + +#if AP_EXTERNAL_AHRS_ADNAV_ENABLED + +#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) + +#include +#include + + +class AP_ExternalAHRS_AdvancedNavigation_Decoder +{ +public: + uint8_t _buffer[AN_DECODE_BUFFER_SIZE]; + uint16_t _buffer_length = 0; + uint64_t _packets_decoded = 0; + uint64_t _bytes_decoded = 0; + uint64_t _bytes_discarded = 0; + uint64_t _lrc_errors = 0; + uint64_t _crc_errors = 0; + size_t _bytes_received = 0; + + 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; + +private: + AP_ExternalAHRS_AdvancedNavigation_Decoder _decoder; + + 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, + 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; + + + + struct PACKED AN_ACKNOWLEGE { + 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_PERIOD { + uint8_t id; + uint32_t packet_period; + }; + + struct PACKED AN_PACKETS_PERIOD { + uint8_t permanent; + uint8_t clear_existing_packet_periods; + AN_PERIOD periods[(AN_MAXIMUM_PACKET_SIZE - 2)/sizeof(AN_PERIOD)]; + }; + + 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_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_PACKETS_PERIOD packets_period; + } payload; + + void update_checks(uint8_t header_id, uint8_t header_length) + { + // Update the packet check and header id + crc = crc16_ccitt(payload.raw_packet, header_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* 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 _device_id; + uint32_t _hardware_rev; + + void update_thread(); + bool get_packets(void); + bool request_data(void); + bool sendPacketRequest(void); + bool get_gnss_capability(void) const; + bool get_baro_capability(void) const; + void handle_packet(); +}; + +#endif // AP_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index ef9ef52158833..b244fbe02786f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -21,3 +21,8 @@ #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED #define AP_EXTERNAL_AHRS_VECTORNAV_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 +