diff --git a/.gitmodules b/.gitmodules index e486e1c618..77786170cd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -21,7 +21,7 @@ url = https://github.com/ArduPilot/gsoap [submodule "modules/DroneCAN/DSDL"] path = modules/DroneCAN/DSDL - url = https://github.com/DroneCAN/DSDL.git + url = https://github.com/CarbonixUAV/DSDL.git [submodule "modules/CrashDebug"] path = modules/CrashDebug url = https://github.com/ardupilot/CrashDebug diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 5b09c6c185..a98f69fa24 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1660,6 +1660,22 @@ void AP_Periph_FW::esc_telem_update() CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], total_size); + + uavcan_equipment_esc_StatusExtended pkt2{}; + pkt2.esc_index = pkt.esc_index; + bool has_ext_data = esc_telem.get_input_duty(i, pkt2.input_pct) + | esc_telem.get_output_duty(i, pkt2.output_pct) + | esc_telem.get_flags(i, pkt2.status_flags); + + if (has_ext_data) { + uint8_t buffer_ext[UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE] {}; + uint16_t total_size_ext = uavcan_equipment_esc_StatusExtended_encode(&pkt2, buffer_ext, !periph.canfdout()); + canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE, + UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer_ext[0], + total_size_ext); + } } } #endif // HAL_WITH_ESC_TELEM diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index f86fd034a0..e48b551ac8 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -151,12 +151,23 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @RebootRequired: True AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0), +#if HAL_WITH_ESC_TELEM + // @Param: EXTLM + // @DisplayName: Enable extended BLHeli telemetry + // @Description: When set to 1, this listens for 13-byte extended telemetry packets from BLHeli ESCs + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("EXTLM", 13, AP_BLHeli, extended_telemetry, 0), +#endif + AP_GROUPEND }; #define RPM_SLEW_RATE 50 AP_BLHeli *AP_BLHeli::_singleton; +uint8_t AP_BLHeli::telem_packet_size; // constructor AP_BLHeli::AP_BLHeli(void) @@ -1415,6 +1426,8 @@ void AP_BLHeli::init(void) telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0); } } + + telem_packet_size = extended_telemetry ? 13 : 10; } /* @@ -1423,9 +1436,9 @@ void AP_BLHeli::init(void) void AP_BLHeli::read_telemetry_packet(void) { #if HAL_WITH_ESC_TELEM - uint8_t buf[telem_packet_size]; + uint8_t buf[13]; if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) { - // short read, we should have 10 bytes ready when this function is called + // short read, we should have all bytes ready when this function is called return; } @@ -1447,18 +1460,28 @@ void AP_BLHeli::read_telemetry_packet(void) hal.rcout->set_active_escs_mask(1<= 2) { uint16_t trpm = new_rpm; @@ -1470,10 +1493,10 @@ void AP_BLHeli::read_telemetry_packet(void) } DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n", last_telem_esc, - t.temperature_cdeg, - t.voltage, - t.current, - t.consumption_mah, + telemetryData.temperature_cdeg, + telemetryData.voltage, + telemetryData.current, + telemetryData.consumption_mah, trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis()); } #endif // HAL_WITH_ESC_TELEM diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 2614cd39ae..b201eb8b49 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -76,6 +76,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend { AP_Int8 motor_poles; // mask of channels with bi-directional dshot enabled AP_Int32 channel_bidir_dshot_mask; + AP_Int8 extended_telemetry; enum mspState { MSP_IDLE=0, @@ -250,7 +251,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend { // when did we last request telemetry? uint32_t last_telem_request_us; uint8_t last_telem_esc; - static const uint8_t telem_packet_size = 10; + static uint8_t telem_packet_size; bool telem_uart_started; uint32_t last_telem_byte_read_us; int8_t last_control_port; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index b0ee54c5d9..e0d6a47e86 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -290,6 +290,42 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const return true; } +// 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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) { + return false; + } + input_duty = _telem_data[esc_index].input_duty; + return true; +} + +// 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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) { + return false; + } + output_duty = _telem_data[esc_index].output_duty; + return true; +} + +// 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 +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) { + return false; + } + flags = _telem_data[esc_index].flags; + return true; +} + // send ESC telemetry messages over MAVLink void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) { @@ -435,6 +471,15 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { _telem_data[esc_index].usage_s = new_data.usage_s; } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) { + _telem_data[esc_index].input_duty = new_data.input_duty; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) { + _telem_data[esc_index].output_duty = new_data.output_duty; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) { + _telem_data[esc_index].flags = new_data.flags; + } _telem_data[esc_index].count++; _telem_data[esc_index].types |= data_mask; @@ -507,6 +552,28 @@ void AP_ESC_Telem::update() error_rate : _rpm_data[i].error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); + + // Write ESC extended status messages + // id: starts from 0 + // input duty: duty cycle input to the ESC in percent + // output duty: duty cycle output to the motor in percent + // status flags: manufacurer-specific status flags + bool has_ext_data = _telem_data[i].types & + (AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY | + 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 : _telem_data[i].input_duty, + output_duty : _telem_data[i].output_duty, + flags : _telem_data[i].flags, + }; + AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); + } + _last_telem_log_ms[i] = _telem_data[i].last_update_ms; _last_rpm_log_us[i] = _rpm_data[i].last_update_us; } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index e0a8276426..2ed1b87a83 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -66,6 +66,15 @@ 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; + // 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; + + // get an individual ESC's output duty if available, returns true on success + bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const; + + // get an individual ESC's status flags if available, returns true on success + bool get_flags(uint8_t esc_index, uint32_t& flags) const; + // 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 index 769807230f..fc52489411 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -25,6 +25,9 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated + uint8_t input_duty; // input duty cycle + uint8_t output_duty; // output duty cycle + uint32_t flags; // Status flags }; struct RpmData { @@ -41,7 +44,13 @@ class AP_ESC_Telem_Backend { VOLTAGE = 1 << 2, CURRENT = 1 << 3, CONSUMPTION = 1 << 4, - USAGE = 1 << 5 + USAGE = 1 << 5, + // 6 reserved for temperature external + // 7 reserved for motor temperature external + // 8 and 9 are reserved for recent dshot extensions + INPUT_DUTY = 1 << 10, + OUTPUT_DUTY = 1 << 11, + FLAGS = 1 << 12 }; diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index 89d0578875..3d0c6f4ef3 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -3,7 +3,8 @@ #include #define LOG_IDS_FROM_ESC_TELEM \ - LOG_ESC_MSG + LOG_ESC_MSG, \ + LOG_ESCX_MSG // @LoggerMessage: ESC // @Description: Feedback received from ESCs @@ -31,6 +32,24 @@ struct PACKED log_Esc { float error_rate; }; +// @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", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \ + { LOG_ESCX_MSG, sizeof(log_EscX), \ + "ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true }, diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat index f46e5137da..614a32263b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -120,4 +120,6 @@ define HAL_PERIPH_ENABLE_RC_OUT # enable ESC control define HAL_SUPPORT_RCOUT_SERIAL 1 -define HAL_PERIPH_ARM_MONITORING_ENABLE 1 \ No newline at end of file +define HAL_PERIPH_ARM_MONITORING_ENABLE 1 + +define NUM_SERVO_CHANNELS 2 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index d2c089ec61..0d936b08c4 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -204,6 +205,10 @@ static uavcan::Subscriber *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +// handler Extended ESC status +UC_REGISTRY_BINDER(ESCXStatusCb, uavcan::equipment::esc::StatusExtended); +static uavcan::Subscriber *extesc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + // handler DEBUG UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; @@ -432,6 +437,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status)); } + extesc_status_listener[driver_index] = new uavcan::Subscriber(*_node); + if (extesc_status_listener[driver_index]) { + extesc_status_listener[driver_index]->start(ESCXStatusCb(this, &handle_esc_ext_status)); + } + debug_listener[driver_index] = new uavcan::Subscriber(*_node); if (debug_listener[driver_index]) { debug_listener[driver_index]->start(DebugCb(this, &handle_debug)); @@ -1073,6 +1083,34 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E #endif } +/* + handle ESC status extended message + */ +void AP_UAVCAN::handle_esc_ext_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCXStatusCb &cb) +{ +#if HAL_WITH_ESC_TELEM + const uint8_t esc_offset = constrain_int16(ap_uavcan->_esc_offset.get(), 0, UAVCAN_SRV_NUMBER); + const uint8_t esc_index = cb.msg->esc_index + esc_offset; + + if (!is_esc_data_index_valid(esc_index)) { + return; + } + + TelemetryData telemetryData { + .motor_temp_cdeg = (int16_t)(cb.msg->motor_temperature_degC * 100), + .input_duty = cb.msg->input_pct, + .output_duty = cb.msg->output_pct, + .flags = (uint32_t)cb.msg->status_flags, + }; + + ap_uavcan->update_telem_data(esc_index, telemetryData, + AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE + | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY + | AP_ESC_Telem_Backend::TelemetryType::FLAGS); +#endif +} + bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) { if (index > UAVCAN_SRV_NUMBER) { // printf("UAVCAN: invalid esc index: %d. max index allowed: %d\n\r", index, UAVCAN_SRV_NUMBER); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index e7e4624658..9d254d2f6c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -51,6 +51,7 @@ class ActuatorStatusCb; class PowerCktStatusCb; class DeviceTemperatureCb; class ESCStatusCb; +class ESCXStatusCb; class DebugCb; class ParamGetSetCb; class ParamExecuteOpcodeCb; @@ -346,6 +347,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { static void handle_power_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PowerCktStatusCb &cb); static void handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb); static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb); + static void handle_esc_ext_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCXStatusCb &cb); static bool is_esc_data_index_valid(const uint8_t index); static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb); static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb); diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index f8cc245a8a..eec585580b 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit f8cc245a8afafd21b99d20bab7f04aeac3c30fff +Subproject commit eec585580b3351a19bdfac26028ce24de012edcf