AP_Periph: add a way to transmit MovingBaseline Data on another port

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
 

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

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

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

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);