AP_DroneCAN: make himark servo support optional

tridge · Sep 4, 2023 · 37e34b3 · 37e34b3
1 parent 337ba27
commit 37e34b3
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 14 deletions.
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -280,13 +280,15 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
     // esc_raw is one higher than high priority to ensure that it is given higher priority over act_out_array
     esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH - 1);
 
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
     esc_hobbywing_raw.set_timeout_ms(2);
     esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
 #endif
-
+
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
     himark_out.set_timeout_ms(2);
     himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
+#endif
 
     rgb_led.set_timeout_ms(20);
     rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
@@ -397,7 +399,7 @@ void AP_DroneCAN::loop(void)
         }
 #endif
         logging();
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
         hobbywing_ESC_update();
 #endif
         if (_SRV_armed_mask != 0) {
@@ -406,9 +408,12 @@ void AP_DroneCAN::loop(void)
             const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
             if (now - _SRV_last_send_us >= servo_period_us) {
                 _SRV_last_send_us = now;
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
                 if (option_is_set(Options::USE_HIMARK_SERVO)) {
                     SRV_send_himark();
-                } else {
+                } else
+#endif
+                {
                     SRV_send_actuator();
                 }
                 for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
@@ -419,7 +424,7 @@ void AP_DroneCAN::loop(void)
     }
 }
 
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
 void AP_DroneCAN::hobbywing_ESC_update(void)
 {
     uint32_t now = AP_HAL::millis();
@@ -492,7 +497,7 @@ void AP_DroneCAN::handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer,
     }
 
 }
-#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#endif // AP_DRONECAN_HOBBYWING_ESC_ENABLED
 
 void AP_DroneCAN::send_node_status(void)
 {
@@ -622,6 +627,7 @@ void AP_DroneCAN::SRV_send_actuator(void)
     } while (repeat_send);
 }
 
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
 /*
   Himark servo output. This uses com.himark.servo.ServoCmd packets
  */
@@ -653,6 +659,7 @@ void AP_DroneCAN::SRV_send_himark(void)
 
     himark_out.broadcast(msg);
 }
+#endif // AP_DRONECAN_HIMARK_SERVO_ENABLED
 
 void AP_DroneCAN::SRV_send_esc(void)
 {
@@ -703,7 +710,7 @@ void AP_DroneCAN::SRV_send_esc(void)
     }
 }
 
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
 /*
   support for Hobbywing DroneCAN ESCs
  */
@@ -751,7 +758,7 @@ void AP_DroneCAN::SRV_send_esc_hobbywing(void)
         canard_iface.processTx(true);
     }
 }
-#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#endif // AP_DRONECAN_HOBBYWING_ESC_ENABLED
 
 void AP_DroneCAN::SRV_push_servos()
 {
@@ -785,7 +792,7 @@ void AP_DroneCAN::SRV_push_servos()
 
     if (_ESC_armed_mask != 0) {
         // push ESCs as fast as we can
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
         if (option_is_set(Options::USE_HOBBYWING_ESC)) {
             SRV_send_esc_hobbywing();
         } else
@@ -1192,6 +1199,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const
                                    0, 0, 0, 0, 0, 0);
 }
 
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
 /*
   handle himark ServoInfo message
  */
@@ -1211,6 +1219,7 @@ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, cons
                                    msg.pcb_temp*0.2-40,
                                    msg.error_status);
 }
+#endif // AP_DRONECAN_HIMARK_SERVO_ENABLED
 
 #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
 void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -50,8 +50,12 @@
 #define AP_DRONECAN_HW_VERS_MINOR 0
 
 
-#ifndef AP_DRONECAN_HOBBYWING_ESC_SUPPORT
-#define AP_DRONECAN_HOBBYWING_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
+#ifndef AP_DRONECAN_HOBBYWING_ESC_ENABLED
+#define AP_DRONECAN_HOBBYWING_ESC_ENABLED (BOARD_FLASH_SIZE>1024)
+#endif
+
+#ifndef AP_DRONECAN_HIMARK_SERVO_ENABLED
+#define AP_DRONECAN_HIMARK_SERVO_ENABLED (BOARD_FLASH_SIZE>1024)
 #endif
 
 // fwd-declare callback classes
@@ -145,7 +149,9 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     ///// SRV output /////
     void SRV_send_actuator();
     void SRV_send_esc();
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
     void SRV_send_himark();
+#endif
 
     //scale servo output appropriately before sending
     int16_t scale_esc_output(uint8_t idx);
@@ -253,7 +259,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
     Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
     Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
+
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
     Canard::Publisher<com_himark_servo_ServoCmd> himark_out{canard_iface};
+#endif
 
 #if AP_DRONECAN_SEND_GPS
     Canard::Publisher<uavcan_equipment_gnss_Fix2> gnss_fix2{canard_iface};
@@ -297,7 +306,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
     uavcan_protocol_GetNodeInfoResponse node_info_rsp;
 
-#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#if AP_DRONECAN_HOBBYWING_ESC_ENABLED
     /*
       Hobbywing ESC support. Note that we need additional meta-data as
       the status messages do not have an ESC ID in them, so we need a
@@ -323,15 +332,18 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     void handle_hobbywing_GetEscID(const CanardRxTransfer& transfer, const com_hobbywing_esc_GetEscID& msg);
     void handle_hobbywing_StatusMsg1(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg1& msg);
     void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
-#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
+#endif // AP_DRONECAN_HOBBYWING_ESC_ENABLED
+
+#if AP_DRONECAN_HIMARK_SERVO_ENABLED
+    void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
+#endif
 
     // incoming button handling
     void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
     void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
     void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
     void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
     void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
-    void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
     static bool is_esc_data_index_valid(const uint8_t index);
     void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
     void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);