From e346eb814ce32dfc734f7496fbe48692c5d357d5 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 28 Feb 2023 14:50:23 -0800 Subject: [PATCH] AP_UAVCAN: add GPS-out --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 194 +++++++++++++++++++++++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 18 +++ 2 files changed, 210 insertions(+), 2 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 780b1cd726..f66341f0e6 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -42,6 +42,12 @@ #include #include #include +#if AP_DRONECAN_SEND_GPS +#include +#include +#include +#include +#endif #include #include @@ -50,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -132,7 +139,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: OPTION // @DisplayName: UAVCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), @@ -172,6 +179,12 @@ static uavcan::Publisher* rgb_led[ static uavcan::Publisher* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +#if AP_DRONECAN_SEND_GPS +static uavcan::Publisher* gnss_fix2[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* gnss_auxiliary[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* gnss_heading[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::Publisher* gnss_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +#endif static uavcan::Publisher* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS]; static uavcan::Publisher* notify_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; @@ -256,7 +269,7 @@ bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface) { } #pragma GCC diagnostic push -#pragma GCC diagnostic error "-Wframe-larger-than=1700" +#pragma GCC diagnostic error "-Wframe-larger-than=2200" void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) { _driver_index = driver_index; @@ -400,6 +413,24 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) arming_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); arming_status[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); +#if AP_DRONECAN_SEND_GPS + gnss_fix2[driver_index] = new uavcan::Publisher(*_node); + gnss_fix2[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + gnss_fix2[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + + gnss_auxiliary[driver_index] = new uavcan::Publisher(*_node); + gnss_auxiliary[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + gnss_auxiliary[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + + gnss_heading[driver_index] = new uavcan::Publisher(*_node); + gnss_heading[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + gnss_heading[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + + gnss_status[driver_index] = new uavcan::Publisher(*_node); + gnss_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + gnss_status[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); +#endif + rtcm_stream[driver_index] = new uavcan::Publisher(*_node); rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); @@ -521,6 +552,15 @@ void AP_UAVCAN::loop(void) #if AP_OPENDRONEID_ENABLED AP::opendroneid().dronecan_send(this); #endif + +#if AP_DRONECAN_SEND_GPS + if (option_is_set(AP_UAVCAN::Options::SEND_GNSS) && !AP_GPS_UAVCAN::instance_exists(this)) { + // send if enabled and this interface/driver is not used by the AP_GPS driver + gnss_send_fix(); + gnss_send_yaw(); + } +#endif + logging(); } } @@ -830,6 +870,156 @@ void AP_UAVCAN::notify_state_send() _last_notify_state_ms = AP_HAL::native_millis(); } +#if AP_DRONECAN_SEND_GPS +void AP_UAVCAN::gnss_send_fix() +{ + const AP_GPS &gps = AP::gps(); + + const uint32_t gps_lib_time_ms = gps.last_message_time_ms(); + if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) { + return; + } + _gnss.last_gps_lib_fix_ms = gps_lib_time_ms; + + /* + send Fix2 packet + */ + + uavcan::equipment::gnss::Fix2 pkt {}; + const Location &loc = gps.location(); + const Vector3f &vel = gps.velocity(); + + pkt.timestamp.usec = AP_HAL::native_micros64(); + pkt.gnss_timestamp.usec = gps.time_epoch_usec(); + if (pkt.gnss_timestamp.usec == 0) { + pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_NONE; + } else { + pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC; + } + pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; + pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; + pkt.height_ellipsoid_mm = loc.alt * 10; + pkt.height_msl_mm = loc.alt * 10; + for (uint8_t i=0; i<3; i++) { + pkt.ned_velocity[i] = vel[i]; + } + pkt.sats_used = gps.num_sats(); + switch (gps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + case AP_GPS::GPS_Status::NO_FIX: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_NO_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_2D_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_SBAS; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; + pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; + pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED; + break; + } + + pkt.covariance.resize(6); + float hacc; + if (gps.horizontal_accuracy(hacc)) { + pkt.covariance[0] = pkt.covariance[1] = sq(hacc); + } + float vacc; + if (gps.vertical_accuracy(vacc)) { + pkt.covariance[2] = sq(vacc); + } + float sacc; + if (gps.speed_accuracy(sacc)) { + const float vc3 = sq(sacc); + pkt.covariance[3] = pkt.covariance[4] = pkt.covariance[5] = vc3; + } + + gnss_fix2[_driver_index]->broadcast(pkt); + + + + const uint32_t now_ms = AP_HAL::native_millis(); + if (now_ms - _gnss.last_send_status_ms >= 1000) { + _gnss.last_send_status_ms = now_ms; + + /* + send aux packet + */ + uavcan::equipment::gnss::Auxiliary pkt_auxiliary {}; + pkt_auxiliary.hdop = gps.get_hdop() * 0.01; + pkt_auxiliary.vdop = gps.get_vdop() * 0.01; + + gnss_auxiliary[_driver_index]->broadcast(pkt_auxiliary); + + + /* + send Status packet + */ + ardupilot::gnss::Status pkt_status {}; + pkt_status.healthy = gps.is_healthy(); + if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { + pkt_status.status |= ardupilot::gnss::Status::STATUS_LOGGING; + } + uint8_t idx; // unused + if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) { + pkt_status.status |= ardupilot::gnss::Status::STATUS_ARMABLE; + } + + uint32_t error_codes; + if (gps.get_error_codes(error_codes)) { + pkt_status.error_codes = error_codes; + } + + gnss_status[_driver_index]->broadcast(pkt_status); + } +} + +void AP_UAVCAN::gnss_send_yaw() +{ + const AP_GPS &gps = AP::gps(); + + if (!gps.have_gps_yaw()) { + return; + } + + float yaw_deg, yaw_acc_deg; + uint32_t yaw_time_ms; + if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) { + return; + } + + _gnss.last_lib_yaw_time_ms = yaw_time_ms; + + ardupilot::gnss::Heading pkt_heading {}; + pkt_heading.heading_valid = true; + pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg); + pkt_heading.heading_rad = radians(yaw_deg); + pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg); + + gnss_heading[_driver_index]->broadcast(pkt_heading); +} +#endif // AP_DRONECAN_SEND_GPS + + void AP_UAVCAN::rtcm_stream_send() { WITH_SEMAPHORE(_rtcm_stream.sem); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 5cd5f7da66..cf11dcf773 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -36,6 +36,10 @@ #define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS #endif +#ifndef AP_DRONECAN_SEND_GPS +#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024) +#endif + #define AP_UAVCAN_SW_VERS_MAJOR 1 #define AP_UAVCAN_SW_VERS_MINOR 0 @@ -205,6 +209,7 @@ public: CANFD_ENABLED = (1U<<2), DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), USE_ACTUATOR_PWM = (1U<<4), + SEND_GNSS = (1U<<5), }; // check if a option is set @@ -326,6 +331,19 @@ private: uint8_t pending_mask; // mask of interfaces to send to } _buzzer; +#if AP_DRONECAN_SEND_GPS + // send GNSS Fix and yaw, same thing AP_GPS_UAVCAN would receive + void gnss_send_fix(); + void gnss_send_yaw(); + + // GNSS Fix and Status + struct { + uint32_t last_gps_lib_fix_ms; + uint32_t last_send_status_ms; + uint32_t last_lib_yaw_time_ms; + } _gnss; +#endif + // GNSS RTCM injection struct { HAL_Semaphore sem;