Fix review comments

wip
ArduPilot · Jul 23, 2024 · cdfb37d · cdfb37d
1 parent 8195e84
commit cdfb37d
Show file tree

Hide file tree

Showing 9 changed files with 52 additions and 51 deletions.
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
@@ -121,6 +121,8 @@ def __init__(self,
     Feature('ESC', 'PICCOLOCAN', 'HAL_PICCOLO_CAN_ENABLE', 'Enable PiccoloCAN', 0, None),
     Feature('ESC', 'TORQEEDO', 'HAL_TORQEEDO_ENABLED', 'Enable Torqeedo Motors', 0, None),
 
+    Feature('ESC', 'ESC_EXTENDED_TELM', 'AP_EXTENDED_ESC_TELEM_ENABLED', 'Enable Extended ESC Telem', 0, 'DroneCAN'),
+
     Feature('AP_Periph', 'LONG_TEXT', 'HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF', 'Enable extended length text strings', 0, None),
 
     Feature('Camera', 'Camera', 'AP_CAMERA_ENABLED', 'Enable Camera Trigger support', 0, None),

diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
@@ -62,6 +62,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('HAL_EFI_ENABLED', 'AP_EFI::AP_EFI',),
             ('AP_EFI_{type}_ENABLED', 'AP_EFI_(?P<type>.*)::update',),
 
+            ('AP_EXTENDED_ESC_TELEM_ENABLED', 'AP_Extended_Telem::get_input_duty',),
+
             ('AP_TEMPERATURE_SENSOR_ENABLED', 'AP_TemperatureSensor::AP_TemperatureSensor',),
             ('AP_TEMPERATURE_SENSOR_{type}_ENABLED', 'AP_TemperatureSensor_(?P<type>.*)::update',),
 

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -1448,12 +1448,13 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
 #endif
 }
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
 /*
   handle Extended ESC status message
  */
 void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
 {
-#if HAL_WITH_ESC_TELEM && AP_EXTENDED_ESC_TELEM_ENABLE
+#if HAL_WITH_ESC_TELEM
     const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
     const uint8_t esc_index = msg.esc_index + esc_offset;
 
@@ -1473,8 +1474,9 @@ void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const
         | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
         | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
         | AP_ESC_Telem_Backend::TelemetryType::FLAGS);
-#endif
+#endif // HAL_WITH_ESC_TELEM
 }
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
 
 bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
     if (index > DRONECAN_SRV_NUMBER) {

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -326,8 +326,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
     Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
 
+#if AP_EXTENDED_ESC_TELEM_ENABLED
     Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status};
     Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
+#endif
 
     Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
     Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
@@ -390,7 +392,9 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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);
+#if AP_EXTENDED_ESC_TELEM_ENABLED
     void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);
+#endif
     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);

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
@@ -305,12 +305,13 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
     return true;
 }
 
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
 // get an individual ESC's input duty if available, returns true on success
 bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const
 {
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
     if (esc_index >= ESC_TELEM_MAX_ESCS
-        || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
+        || telemdata.stale()
         || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) {
         return false;
     }
@@ -321,8 +322,9 @@ bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const
 // get an individual ESC's output duty if available, returns true on success
 bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const
 {
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
     if (esc_index >= ESC_TELEM_MAX_ESCS
-        || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
+        || telemdata.stale()
         || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) {
         return false;
     }
@@ -333,15 +335,16 @@ bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) cons
 // get an individual ESC's status flags if available, returns true on success
 bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
 {
+    const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
     if (esc_index >= ESC_TELEM_MAX_ESCS
-        || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
+        || telemdata.stale()
         || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) {
         return false;
     }
     flags = _telem_data[esc_index].flags;
     return true;
 }
-#endif // AP_EXTENDED_ESC_TELEM_ENABLE
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
 
 // send ESC telemetry messages over MAVLink
 void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
@@ -506,7 +509,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
         telemdata.usage_s = new_data.usage_s;
     }
 
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
     if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) {
         _telem_data[esc_index].input_duty = new_data.input_duty;
     }
@@ -516,7 +519,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
     if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) {
         _telem_data[esc_index].flags = new_data.flags;
     }
-#endif //AP_EXTENDED_ESC_TELEM_ENABLE
+#endif //AP_EXTENDED_ESC_TELEM_ENABLED
 
 #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
     if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) {
@@ -658,7 +661,7 @@ void AP_ESC_Telem::update()
                 };
                 AP::logger().WriteBlock(&pkt, sizeof(pkt));
 
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
                 // Write ESC extended status messages
                 //   id: starts from 0
                 //   input duty: duty cycle input to the ESC in percent
@@ -669,17 +672,25 @@ void AP_ESC_Telem::update()
                          AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY |
                          AP_ESC_Telem_Backend::TelemetryType::FLAGS);
                 if (has_ext_data) {
-                    const struct log_EscX pkt2{
-                        LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESCX_MSG)),
-                        time_us     : AP_HAL::micros64(),
-                        instance    : i,
-                        input_duty  : telemdata.input_duty,
-                        output_duty : telemdata.output_duty,
-                        flags       : telemdata.flags,
-                    };
-                    AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+                    // @LoggerMessage: ESCX
+                    // @Description: ESC extended telemetry data
+                    // @Field: TimeUS: Time since system startup
+                    // @Field: Instance: starts from 0
+                    // @Field: inpct: input duty cycle in percent
+                    // @Field: outpct: output duty cycle in percent
+                    // @Field: flags: manufacturer-specific status flags
+                    AP::logger().WriteStreaming("ESCX",
+                                                "TimeUS,Instance,inpct,outpct,flags",
+                                                "s#%%-",
+                                                "F----",
+                                                "QBBBI",
+                                                AP_HAL::micros64(),
+                                                uint8_t(i),
+                                                uint8_t(telemdata.input_duty),
+                                                uint8_t(telemdata.output_duty),
+                                                uint32_t(telemdata.flags));
                 }
-#endif //AP_EXTENDED_ESC_TELEM_ENABLE
+#endif //AP_EXTENDED_ESC_TELEM_ENABLED
                 _last_telem_log_ms[i] = telemdata.last_update_ms;
                 _last_rpm_log_us[i] = rpmdata.last_update_us;
             }

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h
@@ -67,7 +67,7 @@ class AP_ESC_Telem {
     // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
     bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
 
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
     // get an individual ESC's input duty if available, returns true on success
     bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const;
 
@@ -76,7 +76,7 @@ class AP_ESC_Telem {
 
     // get an individual ESC's status flags if available, returns true on success
     bool get_flags(uint8_t esc_index, uint32_t& flags) const;
-#endif // AP_EXTENDED_ESC_TELEM_ENABLE
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
 
     // return the average motor frequency in Hz for dynamic filtering
     float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
@@ -23,11 +23,11 @@ class AP_ESC_Telem_Backend {
         uint16_t edt2_status;       // status reported by Extended DShot Telemetry v2
         uint16_t edt2_stress;       // stress reported in dedicated messages by Extended DShot Telemetry v2
 #endif
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
         uint8_t input_duty;         // input duty cycle
         uint8_t output_duty;        // output duty cycle
         uint32_t flags;              // Status flags
-#endif // AP_EXTENDED_ESC_TELEM_ENABLE
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
 
         // return true if the data is stale
         bool stale(uint32_t now_ms=0) const volatile;
@@ -55,11 +55,11 @@ class AP_ESC_Telem_Backend {
         EDT2_STATUS = 1 << 8,
         EDT2_STRESS = 1 << 9,
 #endif
-#if AP_EXTENDED_ESC_TELEM_ENABLE
+#if AP_EXTENDED_ESC_TELEM_ENABLED
         INPUT_DUTY  = 1 << 10,
         OUTPUT_DUTY = 1 << 11,
         FLAGS       = 1 << 12
-#endif // AP_EXTENDED_ESC_TELEM_ENABLE
+#endif // AP_EXTENDED_ESC_TELEM_ENABLED
     };
 
 

diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h
@@ -4,8 +4,7 @@
 
 #define LOG_IDS_FROM_ESC_TELEM                  \
     LOG_ESC_MSG,                                \
-    LOG_EDT2_MSG,                               \
-    LOG_ESCX_MSG
+    LOG_EDT2_MSG
 
 // @LoggerMessage: ESC
 // @Description: Feedback received from ESCs
@@ -59,27 +58,8 @@ struct PACKED log_Edt2 {
     uint8_t status;
 };
 
-// @LoggerMessage: ESCX
-// @Description: Extended telemetry feedback received from ESCs
-// @Field: TimeUS: microseconds since system startup
-// @Field: Instance: ESC instance number
-// @Field: inpct: input duty cycle
-// @Field: outpct: output duty cycle
-// @Field: flags: manufacturer-specific status flags
-struct PACKED log_EscX {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    uint8_t instance;
-    uint8_t input_duty;
-    uint8_t output_duty;
-    uint32_t flags;
-};
-
 #define LOG_STRUCTURE_FROM_ESC_TELEM \
     { LOG_ESC_MSG, sizeof(log_Esc), \
-      "ESC",  "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \
+      "ESC",  "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, \
     { LOG_EDT2_MSG, sizeof(log_Edt2), \
-      "EDT2",  "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, \
-    { LOG_ESCX_MSG, sizeof(log_EscX), \
-      "ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true },
-
+      "EDT2",  "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true },
diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h
@@ -188,8 +188,8 @@
 #define AP_EXTENDED_DSHOT_TELEM_V2_ENABLED HAL_REQUIRES_BDSHOT_SUPPORT
 #endif
 
-#ifndef AP_EXTENDED_ESC_TELEM_ENABLE
-#define AP_EXTENDED_ESC_TELEM_ENABLE 0
+#ifndef AP_EXTENDED_ESC_TELEM_ENABLED
+#define AP_EXTENDED_ESC_TELEM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
 // this is used as a general mechanism to make a 'small' build by