From aa9a40acf55ae41eddae239ee64b93b20857e856 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 16 Jul 2021 21:46:24 +0530 Subject: [PATCH] AP_Periph: add a way to transmit MovingBaseline Data on another port --- Tools/AP_Periph/AP_Periph.cpp | 2 - Tools/AP_Periph/AP_Periph.h | 9 ++- Tools/AP_Periph/Parameters.cpp | 10 +++ Tools/AP_Periph/Parameters.h | 4 + Tools/AP_Periph/can.cpp | 133 +++++++++++++++++++++++++-------- 5 files changed, 120 insertions(+), 38 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 06097a3bd3..a8417caae4 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -147,8 +147,6 @@ void AP_Periph_FW::init() gps.set_log_gps_bit(MASK_LOG_GPS); #endif gps.init(serial_manager); - gps.set_MBL_data_cb(FUNCTOR_BIND_MEMBER(&AP_Periph_FW::send_moving_baseline_msg, void, const uint8_t*&, uint16_t)); - gps.set_RelPosHeading_cb(FUNCTOR_BIND_MEMBER(&AP_Periph_FW::send_relposheading_msg, void, uint32_t, float, float, float, float)); } #endif diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 3c87519f37..0b685b998a 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -85,10 +85,8 @@ public: void can_update(); void can_mag_update(); void can_gps_update(); - void send_moving_baseline_msg(const uint8_t *&data, uint16_t len); - void send_relposheading_msg(uint32_t timestamp, float reported_heading, - float relative_distance, float relative_down_pos, - float reported_heading_acc); + void send_moving_baseline_msg(); + void send_relposheading_msg(); void can_baro_update(); void can_airspeed_update(); void can_rangefinder_update(); @@ -111,6 +109,9 @@ public: #ifdef HAL_PERIPH_ENABLE_GPS AP_GPS gps; +#if HAL_NUM_CAN_IFACES >= 2 + int8_t gps_mb_can_port = -1; +#endif #endif #ifdef HAL_PERIPH_ENABLE_MAG diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 5082caa917..a8674eafcc 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -162,6 +162,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @User: Advanced // @RebootRequired: True GSCALAR(gps_port, "GPS_PORT", HAL_PERIPH_GPS_PORT_DEFAULT), + +#if HAL_NUM_CAN_IFACES >= 2 + // @Param: MB_CAN_PORT + // @DisplayName: Moving Baseline CAN Port option + // @Description: Autoselect dedicated CAN port on which moving baseline data will be transmitted. + // @Values: 0:Sends moving baseline data on all ports,1:auto select remaining port for transmitting Moving baseline Data + // @User: Advanced + // @RebootRequired: True + GSCALAR(gps_mb_only_can_port, "GPS_MB_ONLY_PORT", 0), +#endif #endif #ifdef HAL_PERIPH_ENABLE_BATTERY diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 478609b02f..65bff7d9d6 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -48,6 +48,7 @@ public: k_param_can_protocol2, k_param_sysid_this_mav, k_param_serial_manager, + k_param_gps_mb_only_can_port, }; AP_Int16 format_version; @@ -92,6 +93,9 @@ public: #ifdef HAL_PERIPH_ENABLE_GPS AP_Int8 gps_port; +#if HAL_NUM_CAN_IFACES >= 2 + AP_Int8 gps_mb_only_can_port; +#endif #endif #ifdef HAL_PERIPH_ENABLE_MSP diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index ae2a04cecc..d235aed718 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -82,6 +82,14 @@ extern AP_Periph_FW periph; #define HAL_CAN_POOL_SIZE 4000 #endif +#define DEBUG_PRINTS 0 + +#if DEBUG_PRINTS + # define Debug(fmt, args ...) do {can_printf(fmt "\n", ## args);} while(0) +#else + # define Debug(fmt, args ...) +#endif + static struct instance_t { uint8_t index; CanardInstance canard; @@ -102,6 +110,29 @@ static struct instance_t { #endif } instances[HAL_NUM_CAN_IFACES]; +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) +static ioline_t can_term_lines[] = { +HAL_GPIO_PIN_TERMCAN1 + +#if HAL_NUM_CAN_IFACES > 2 +#ifdef HAL_GPIO_PIN_TERMCAN2 +,HAL_GPIO_PIN_TERMCAN2 +#else +#error "Only one Can Terminator defined with over two CAN Ifaces" +#endif +#endif + +#if HAL_NUM_CAN_IFACES > 2 +#ifdef HAL_GPIO_PIN_TERMCAN3 +,HAL_GPIO_PIN_TERMCAN3 +#else +#error "Only two Can Terminator defined with three CAN Ifaces" +#endif +#endif + +}; +#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) + #ifndef CAN_APP_NODE_NAME #define CAN_APP_NODE_NAME "org.ardupilot.ap_periph" #endif @@ -490,6 +521,21 @@ static void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* tr canardSetLocalNodeID(ins, allocated_node_id); printf("IF%d Node ID allocated: %d\n", can_ins->index, allocated_node_id); + +#if defined(HAL_PERIPH_ENABLE_GPS) && (HAL_NUM_CAN_IFACES >= 2) && GPS_MOVING_BASELINE + if (periph.g.gps_mb_only_can_port) { + // we need to assign the unallocated port to be used for Moving Baseline only + periph.gps_mb_can_port = (can_ins->index+1)%HAL_NUM_CAN_IFACES; + if (canardGetLocalNodeID(&instances[periph.gps_mb_can_port].canard) == CANARD_BROADCAST_NODE_ID) { + // copy node id from the primary iface + canardSetLocalNodeID(&instances[periph.gps_mb_can_port].canard, allocated_node_id); +#ifdef HAL_GPIO_PIN_TERMCAN1 + // also terminate the line as we don't have any other device on this port + palWriteLine(can_term_lines[periph.gps_mb_can_port], 1); +#endif + } + } +#endif } } @@ -612,6 +658,7 @@ static void handle_MovingBaselineData(CanardInstance* ins, CanardRxTransfer* tra return; } periph.gps.inject_MBL_data(msg.data.data, msg.data.len); + Debug("MovingBaselineData: len=%u\n", msg.data.len); } #endif // GPS_MOVING_BASELINE @@ -1059,13 +1106,18 @@ static void canard_broadcast(uint64_t data_type_signature, if (canardGetLocalNodeID(&ins.canard) == CANARD_BROADCAST_NODE_ID) { continue; } - canardBroadcast(&ins.canard, - data_type_signature, - data_type_id, - &ins.transfer_id, - priority, - payload, - payload_len); +#if defined(HAL_PERIPH_ENABLE_GPS) && HAL_NUM_CAN_IFACES >= 2 + if (ins.index != periph.gps_mb_can_port) +#endif + { + canardBroadcast(&ins.canard, + data_type_signature, + data_type_id, + &ins.transfer_id, + priority, + payload, + payload_len); + } } } @@ -1624,6 +1676,8 @@ void AP_Periph_FW::can_gps_update(void) return; } gps.update(); + send_moving_baseline_msg(); + send_relposheading_msg(); if (last_gps_update_ms == gps.last_message_time_ms()) { return; } @@ -1842,22 +1896,17 @@ void AP_Periph_FW::can_gps_update(void) } -void AP_Periph_FW::send_moving_baseline_msg(const uint8_t *&data, uint16_t len) +void AP_Periph_FW::send_moving_baseline_msg() { #if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE + const uint8_t *data = nullptr; + uint16_t len = 0; + if (!gps.get_RTCMV3(data, len)) { + return; + } if (len == 0 || data == nullptr) { return; } - // static uint32_t data_cnt = 0; - // static uint32_t last_moving_baseline_ms = 0; - // if (AP_HAL::millis() - last_moving_baseline_ms > 1000) { - // can_printf("RATE: %lu bps", data_cnt); - // data_cnt = 0; - // } - // data_cnt += len; - // if (data_cnt > 0) { - // return; - // } // send the packet from Moving Base to be used RelPosHeading calc by GPS module ardupilot_gnss_MovingBaselineData mbldata {}; // get the data from the moving base @@ -1866,22 +1915,44 @@ void AP_Periph_FW::send_moving_baseline_msg(const uint8_t *&data, uint16_t len) mbldata.data.data = (uint8_t*)data; uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer); - canardBroadcast(&canard, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); + +#if HAL_NUM_CAN_IFACES >= 2 + if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { + canardBroadcast(&instances[gps_mb_can_port].canard, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, + &instances[gps_mb_can_port].transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } else +#endif + { + canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, + ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + } + gps.clear_RTCMV3(); #endif // HAL_PERIPH_ENABLE_GPS && GPS_MOVING_BASELINE } -void AP_Periph_FW::send_relposheading_msg(uint32_t timestamp, float reported_heading, - float relative_distance, float relative_down_pos, - float reported_heading_acc) { +void AP_Periph_FW::send_relposheading_msg() { #if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE + float reported_heading; + float relative_distance; + float relative_down_pos; + float reported_heading_acc; + static uint32_t last_timestamp = 0; + uint32_t curr_timestamp = 0; + gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc); + if (last_timestamp == curr_timestamp) { + return; + } + last_timestamp = curr_timestamp; ardupilot_gnss_RelPosHeading relpos {}; - relpos.timestamp.usec = uint64_t(timestamp)*1000LLU; + relpos.timestamp.usec = uint64_t(curr_timestamp)*1000LLU; relpos.reported_heading_deg = reported_heading; relpos.relative_distance_m = relative_distance; relpos.relative_down_pos_m = relative_down_pos; @@ -1889,10 +1960,8 @@ void AP_Periph_FW::send_relposheading_msg(uint32_t timestamp, float reported_hea relpos.reported_heading_acc_available = true; uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE] {}; const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer); - canardBroadcast(&canard, - ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE, + canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE, ARDUPILOT_GNSS_RELPOSHEADING_ID, - &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size);