From 73cd057c98a88b6843a354dddd674687992f1ef8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2020 16:53:49 +1100 Subject: [PATCH 01/10] AP_ESC_Telem: added support for Hobbywing 80A HV Pro ESC telem --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 168 ++++++++++++++++++++++++ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 71 ++++++++++ 2 files changed, 239 insertions(+) create mode 100644 libraries/AP_ESC_Telem/AP_ESC_Telem.cpp create mode 100644 libraries/AP_ESC_Telem/AP_ESC_Telem.h diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp new file mode 100644 index 0000000000..18e94b14c0 --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -0,0 +1,168 @@ +/* + ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be + incorporated into a broader ESC telemetry library in ArduPilot + master in the future + */ +#include "AP_ESC_Telem.h" +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define TELEM_HEADER 0x9B +#define TELEM_LEN 0x16 + +AP_ESC_Telem *AP_ESC_Telem::singleton; + +// constructor +AP_ESC_Telem::AP_ESC_Telem(void) +{ + singleton = this; +} + +void AP_ESC_Telem::init() +{ + AP_SerialManager *serial_manager = AP_SerialManager::get_instance(); + if (!serial_manager) { + return; + } + uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry, 0); + if (uart) { + sem = hal.util->new_semaphore(); + hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_ESC_Telem::timer_update, void)); + } +} + +/* + update ESC telemetry + */ +void AP_ESC_Telem::timer_update() +{ + if (!initialised) { + initialised = true; + uart->begin(19200); + } + + uint32_t n = uart->available(); + if (n == 0) { + return; + } + + // we expect at least 50ms idle between frames + uint32_t now = AP_HAL::millis(); + bool frame_gap = (now - last_read_ms) > 10; + + last_read_ms = now; + + // don't read too much in one loop to prevent too high CPU load + n = MIN(n, 500U); + if (len == 0 && !frame_gap) { + // discard + while (n--) { + uart->read(); + } + return; + } + + if (frame_gap) { + len = 0; + } + + while (n--) { + uint8_t b = uart->read(); + //hal.console->printf("t=%u 0x%02x\n", now, b); + if (len == 0 && b != TELEM_HEADER) { + continue; + } + if (len == 1 && b != TELEM_LEN) { + continue; + } + uint8_t *buf = (uint8_t *)&pkt; + buf[len++] = b; + if (len == sizeof(pkt)) { + parse_packet(); + len = 0; + } + } +} + +static uint16_t calc_crc(const uint8_t *buf, uint8_t len) +{ + uint16_t crc = 0; + while (len--) { + crc += *buf++; + } + return crc; +} + +/* + parse packet + */ +void AP_ESC_Telem::parse_packet(void) +{ + uint16_t crc = calc_crc((uint8_t *)&pkt, sizeof(pkt)-2); + if (crc != pkt.crc) { + return; + } + + sem->take_blocking(); + decoded.counter = be32toh(pkt.counter); + decoded.throttle_req = be16toh(pkt.throttle_req); + decoded.throttle = be16toh(pkt.throttle); + decoded.rpm = be16toh(pkt.rpm); + decoded.voltage = be16toh(pkt.voltage) * 0.1; + decoded.load = be16toh(pkt.load); + decoded.current = be16toh(pkt.current); + decoded.temperature = be16toh(pkt.temperature); + decoded.unknown = be16toh(pkt.unknown); + sem->give(); + +#if 0 + uint32_t now = AP_HAL::millis(); + static uint32_t last_ms; + uint32_t dt = now - last_ms; + last_ms = now; + hal.console->printf("dt=%u %u RPM:%u THR:%u:%u V:%.2f L:%u C:%u\n", dt, + unsigned(decoded.counter), + decoded.rpm, + unsigned(decoded.throttle_req), unsigned(decoded.throttle), + decoded.voltage, unsigned(decoded.load), unsigned(decoded.current)); +#endif + + DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Load,Curr,Temp,Unk", "QIHHHfHHHH", + AP_HAL::micros64(), + decoded.counter, + decoded.rpm, + decoded.throttle_req, decoded.throttle, + decoded.voltage, decoded.load, decoded.current, + decoded.temperature, decoded.unknown); +} + +/* + send telemetry on mavlink + */ +void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t chan) +{ + if (!uart) { + return; + } + uint8_t temperature[4] {}; + uint16_t voltage[4] {}; + uint16_t current[4] {}; + uint16_t totalcurrent[4] {}; + uint16_t rpm[4] {}; + uint16_t count[4] {}; + if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)chan, ESC_TELEMETRY_1_TO_4)) { + return; + } + sem->take_blocking(); + voltage[0] = decoded.voltage * 1000; + current[0] = decoded.current; + rpm[0] = decoded.rpm; + sem->give(); + mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count); +} diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h new file mode 100644 index 0000000000..abbf4c5a5c --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -0,0 +1,71 @@ +/* + ESC Telemetry for Hobbywing Pro 80A HV ESC. This will be + incorporated into a broader ESC telemetry library in ArduPilot + master in the future + */ + +#pragma once + +#include +#include +#include + +class AP_ESC_Telem { +public: + AP_ESC_Telem(); + + /* Do not allow copies */ + AP_ESC_Telem(const AP_ESC_Telem &other) = delete; + AP_ESC_Telem &operator=(const AP_ESC_Telem&) = delete; + + void init(void); + + void send_esc_telemetry_mavlink(uint8_t chan); + + static AP_ESC_Telem *get_singleton(void) { + return singleton; + } + +private: + static AP_ESC_Telem *singleton; + AP_HAL::UARTDriver *uart; + + void timer_update(); + + struct PACKED { + uint8_t header; // 0x9B + uint8_t pkt_len; // 0x16 + uint32_t counter; + uint16_t throttle_req; + uint16_t throttle; + uint16_t rpm; + uint16_t voltage; + uint16_t load; + uint16_t current; + uint16_t temperature; + uint16_t unknown; + uint16_t crc; + } pkt; + + bool initialised; + uint8_t len; + uint32_t last_read_ms; + + struct { + uint32_t counter; + uint16_t throttle_req; + uint16_t throttle; + uint16_t rpm; + float voltage; + uint16_t load; + float current; + uint16_t temperature; + uint16_t unknown; + } decoded; + + uint32_t last_mavlink_ms; + + AP_HAL::Semaphore *sem; + + void parse_packet(void); +}; From fd724f94f0ba4e162da970741fa66bf2879618b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2020 16:54:21 +1100 Subject: [PATCH 02/10] HAL_ChibiOS: HACK to pulldown TELEM2 RX for ESC telem need SERIALn_OPTIONS from master to do properly --- libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat index f267345ce9..0764d8df49 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat @@ -323,7 +323,7 @@ PD6 USART2_RX USART2 # the telem2 USART, also with RTS/CTS available # USART3 serial3 telem2 PD8 USART3_TX USART3 -PD9 USART3_RX USART3 +PD9 USART3_RX USART3 PULLDOWN PD11 USART3_CTS USART3 PD12 USART3_RTS USART3 From 7a90719dfd2702d1286687e66b69bb7580b0f476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2020 16:54:34 +1100 Subject: [PATCH 03/10] GCS_MAVLink: send ESC telem on mavlink --- libraries/GCS_MAVLink/GCS_Common.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 95600676e3..2b95bc7c7d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "GCS.h" @@ -3080,6 +3081,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) blheli->send_esc_telemetry_mavlink(uint8_t(chan)); } #endif + AP_ESC_Telem *esc_telem = AP_ESC_Telem::get_singleton(); + if (esc_telem) { + esc_telem->send_esc_telemetry_mavlink(uint8_t(chan)); + } break; } From c329915f3e91e7e373e576bd028e0a5dd99dde68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2020 16:54:46 +1100 Subject: [PATCH 04/10] Copter: added ESC telemetry --- ArduCopter/Copter.h | 6 +++++- ArduCopter/Parameters.h | 1 - ArduCopter/system.cpp | 3 +++ ArduCopter/wscript | 1 + 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7242bdac9d..c273923243 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -151,6 +151,8 @@ #include #endif +#include + #if OSD_ENABLED == ENABLED #include #endif @@ -466,7 +468,9 @@ class Copter : public AP_HAL::HAL::Callbacks { #if OSD_ENABLED == ENABLED AP_OSD osd; #endif - + + AP_ESC_Telem esc_telem; + // Variables for extended status MAVLink messages uint32_t control_sensors_present; uint32_t control_sensors_enabled; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 2620c5cabe..730b87bdcb 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -589,7 +589,6 @@ class ParametersG2 { // follow AP_Follow follow; #endif - }; /* diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index cfec54d01f..6d61ad05fd 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -284,6 +284,9 @@ void Copter::init_ardupilot() // disable safety if requested BoardConfig.init_safety(); + // init ESC telemetry + esc_telem.init(); + // default enable RC override copter.ap.rc_override_enable = true; diff --git a/ArduCopter/wscript b/ArduCopter/wscript index ef270321a4..0f7d165d02 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -34,6 +34,7 @@ def build(bld): 'AP_Follow', 'AP_Devo_Telem', 'AP_OSD', + 'AP_ESC_Telem', ], ) From fa7d93b26c21be3e16faf76b5593ff6605dccaa1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 Feb 2020 10:43:13 +1100 Subject: [PATCH 05/10] AP_ESC_Telem: fixed ESC current --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 18e94b14c0..9b95c06de4 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -133,12 +133,15 @@ void AP_ESC_Telem::parse_packet(void) decoded.voltage, unsigned(decoded.load), unsigned(decoded.current)); #endif - DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Load,Curr,Temp,Unk", "QIHHHfHHHH", + DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Load,Curr,Temp,Unk", + "QIHHHfHfHH", AP_HAL::micros64(), decoded.counter, decoded.rpm, decoded.throttle_req, decoded.throttle, - decoded.voltage, decoded.load, decoded.current, + decoded.voltage, + decoded.load, + decoded.current, decoded.temperature, decoded.unknown); } From 5e4e638661c7b5d6c6a826c40accf106cf505f4c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 Feb 2020 18:50:23 +1100 Subject: [PATCH 06/10] AP_ESC_Telem: fixed current reporting --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 4 ++-- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 9b95c06de4..54d816da71 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -115,8 +115,8 @@ void AP_ESC_Telem::parse_packet(void) decoded.throttle = be16toh(pkt.throttle); decoded.rpm = be16toh(pkt.rpm); decoded.voltage = be16toh(pkt.voltage) * 0.1; - decoded.load = be16toh(pkt.load); - decoded.current = be16toh(pkt.current); + decoded.current = int16_t(be16toh(pkt.current)) * 0.01; + decoded.load = int16_t(be16toh(pkt.load)); decoded.temperature = be16toh(pkt.temperature); decoded.unknown = be16toh(pkt.unknown); sem->give(); diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index abbf4c5a5c..3ebc5386eb 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -40,8 +40,8 @@ class AP_ESC_Telem { uint16_t throttle; uint16_t rpm; uint16_t voltage; - uint16_t load; - uint16_t current; + int16_t current; + int16_t load; uint16_t temperature; uint16_t unknown; uint16_t crc; @@ -57,8 +57,8 @@ class AP_ESC_Telem { uint16_t throttle; uint16_t rpm; float voltage; - uint16_t load; float current; + int16_t load; uint16_t temperature; uint16_t unknown; } decoded; From 1cc58fe311553379034dfb52f94da06240921eb5 Mon Sep 17 00:00:00 2001 From: Akshay Bajaj Date: Thu, 20 Feb 2020 11:45:27 -0800 Subject: [PATCH 07/10] Removed un-needed pre-arm checks for this configuration --- ArduCopter/AP_Arming.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 8e9a2b69e8..efff904c37 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -531,6 +531,7 @@ bool AP_Arming_Copter::pre_arm_proximity_check(bool display_failure) // has side-effect that logging is started bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) { +/* // always check if inertial nav has started and is ready if (!ahrs.healthy()) { check_failed(ARMING_CHECK_NONE, display_failure, "Waiting for Nav Checks"); @@ -544,7 +545,6 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) return false; } #endif - if (_compass.is_calibrating()) { check_failed(ARMING_CHECK_NONE, display_failure, "Compass calibration running"); return false; @@ -589,6 +589,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) return false; } } +*/ control_mode_t control_mode = copter.control_mode; @@ -709,6 +710,7 @@ bool AP_Arming_Copter::pre_takeoff_checks(void) gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: not armed"); return false; } +/* float pos_change, alt_change; if (!copter.gps.get_pre_arm_pos_change(pos_change, alt_change)) { gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: no GPS data"); @@ -722,5 +724,6 @@ bool AP_Arming_Copter::pre_takeoff_checks(void) gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: alt change %.2f", alt_change); return false; } +*/ return true; } From 46f376afa9d9090f98d2e863f5f5c42799ba3def Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Mar 2020 09:11:22 +1100 Subject: [PATCH 08/10] AP_ESC_Telem: fixed scaling of RPM --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 27 +++++++++++++------------ libraries/AP_ESC_Telem/AP_ESC_Telem.h | 12 +++++------ 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 54d816da71..3bca79d55b 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -113,36 +113,37 @@ void AP_ESC_Telem::parse_packet(void) decoded.counter = be32toh(pkt.counter); decoded.throttle_req = be16toh(pkt.throttle_req); decoded.throttle = be16toh(pkt.throttle); - decoded.rpm = be16toh(pkt.rpm); + decoded.rpm = be16toh(pkt.erpm) * 5.0 / 7.0; decoded.voltage = be16toh(pkt.voltage) * 0.1; decoded.current = int16_t(be16toh(pkt.current)) * 0.01; - decoded.load = int16_t(be16toh(pkt.load)); + decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01; decoded.temperature = be16toh(pkt.temperature); - decoded.unknown = be16toh(pkt.unknown); + decoded.status = be16toh(pkt.status); sem->give(); #if 0 - uint32_t now = AP_HAL::millis(); - static uint32_t last_ms; - uint32_t dt = now - last_ms; - last_ms = now; - hal.console->printf("dt=%u %u RPM:%u THR:%u:%u V:%.2f L:%u C:%u\n", dt, + hal.console->printf("%u RPM:%.0f THR:%u:%u V:%.2f PC:%.2f C:%.2f T:0x%x S:0x%x\n", unsigned(decoded.counter), decoded.rpm, unsigned(decoded.throttle_req), unsigned(decoded.throttle), - decoded.voltage, unsigned(decoded.load), unsigned(decoded.current)); + decoded.voltage, + decoded.phase_current, + decoded.current, + unsigned(decoded.temperature), + unsigned(decoded.status)); #endif - DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Load,Curr,Temp,Unk", - "QIHHHfHfHH", + DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,CurrP,Curr,Temp,Status", + "QIfHHfffHH", AP_HAL::micros64(), decoded.counter, decoded.rpm, decoded.throttle_req, decoded.throttle, decoded.voltage, - decoded.load, + decoded.phase_current, decoded.current, - decoded.temperature, decoded.unknown); + decoded.temperature, + decoded.status); } /* diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 3ebc5386eb..6fc6a2d137 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -38,12 +38,12 @@ class AP_ESC_Telem { uint32_t counter; uint16_t throttle_req; uint16_t throttle; - uint16_t rpm; + uint16_t erpm; uint16_t voltage; int16_t current; - int16_t load; + int16_t phase_current; uint16_t temperature; - uint16_t unknown; + uint16_t status; uint16_t crc; } pkt; @@ -55,12 +55,12 @@ class AP_ESC_Telem { uint32_t counter; uint16_t throttle_req; uint16_t throttle; - uint16_t rpm; + float rpm; float voltage; float current; - int16_t load; + float phase_current; uint16_t temperature; - uint16_t unknown; + uint16_t status; } decoded; uint32_t last_mavlink_ms; From 83883ea150653b486855cfb526fbc39f8e36c0cf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 1 Apr 2020 11:30:15 +1100 Subject: [PATCH 09/10] AP_ESC_Telem: update based on new protocol docs --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 69 ++++++++++++++++++------- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 11 ++-- 2 files changed, 58 insertions(+), 22 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 3bca79d55b..00d3dc0d6f 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -99,6 +99,35 @@ static uint16_t calc_crc(const uint8_t *buf, uint8_t len) return crc; } +static const struct { + uint8_t adc_temp; + uint8_t temp_C; +} temp_table[] = { + { 241, 0}, { 240, 1}, { 239, 2}, { 238, 3}, { 237, 4}, { 236, 5}, { 235, 6}, { 234, 7}, { 233, 8}, { 232, 9}, + { 231, 10}, { 230, 11}, { 229, 12}, { 228, 13}, { 227, 14}, { 226, 15}, { 224, 16}, { 223, 17}, { 222, 18}, { 220, 19}, + { 219, 20}, { 217, 21}, { 216, 22}, { 214, 23}, { 213, 24}, { 211, 25}, { 209, 26}, { 208, 27}, { 206, 28}, { 204, 29}, + { 202, 30}, { 201, 31}, { 199, 32}, { 197, 33}, { 195, 34}, { 193, 35}, { 191, 36}, { 189, 37}, { 187, 38}, { 185, 39}, + { 183, 40}, { 181, 41}, { 179, 42}, { 177, 43}, { 174, 44}, { 172, 45}, { 170, 46}, { 168, 47}, { 166, 48}, { 164, 49}, + { 161, 50}, { 159, 51}, { 157, 52}, { 154, 53}, { 152, 54}, { 150, 55}, { 148, 56}, { 146, 57}, { 143, 58}, { 141, 59}, + { 139, 60}, { 136, 61}, { 134, 62}, { 132, 63}, { 130, 64}, { 128, 65}, { 125, 66}, { 123, 67}, { 121, 68}, { 119, 69}, + { 117, 70}, { 115, 71}, { 113, 72}, { 111, 73}, { 109, 74}, { 106, 75}, { 105, 76}, { 103, 77}, { 101, 78}, { 99, 79}, + { 97, 80}, { 95, 81}, { 93, 82}, { 91, 83}, { 90, 84}, { 88, 85}, { 85, 86}, { 84, 87}, { 82, 88}, { 81, 89}, + { 79, 90}, { 77, 91}, { 76, 92}, { 74, 93}, { 73, 94}, { 72, 95}, { 69, 96}, { 68, 97}, { 66, 98}, { 65, 99}, + { 64, 100}, { 62, 101}, { 62, 102}, { 61, 103}, { 59, 104}, { 58, 105}, { 56, 106}, { 54, 107}, { 54, 108}, { 53, 109}, + { 51, 110}, { 51, 111}, { 50, 112}, { 48, 113}, { 48, 114}, { 46, 115}, { 46, 116}, { 44, 117}, { 43, 118}, { 43, 119}, + { 41, 120}, { 41, 121}, { 39, 122}, { 39, 123}, { 39, 124}, { 37, 125}, { 37, 126}, { 35, 127}, { 35, 128}, { 33, 129}, +}; + +uint8_t AP_ESC_Telem::temperature_decode(uint8_t temp_raw) const +{ + for (uint8_t i=0; itake_blocking(); decoded.counter = be32toh(pkt.counter); - decoded.throttle_req = be16toh(pkt.throttle_req); - decoded.throttle = be16toh(pkt.throttle); - decoded.rpm = be16toh(pkt.erpm) * 5.0 / 7.0; + decoded.throttle_req = be16toh(pkt.throttle_req) * 100.0 / 1024.0; + decoded.throttle = be16toh(pkt.throttle) * 100.0 / 1024.0; + decoded.rpm = be16toh(pkt.erpm) * 10.0 / 14.0; decoded.voltage = be16toh(pkt.voltage) * 0.1; - decoded.current = int16_t(be16toh(pkt.current)) * 0.01; - decoded.phase_current = int16_t(be16toh(pkt.phase_current)) * 0.01; - decoded.temperature = be16toh(pkt.temperature); + decoded.current = int16_t(be16toh(pkt.current)) / 64.0; + decoded.phase_current = int16_t(be16toh(pkt.phase_current)) / 64.0; + decoded.mos_temperature = temperature_decode(pkt.mos_temperature); + decoded.cap_temperature = temperature_decode(pkt.cap_temperature); decoded.status = be16toh(pkt.status); sem->give(); #if 0 - hal.console->printf("%u RPM:%.0f THR:%u:%u V:%.2f PC:%.2f C:%.2f T:0x%x S:0x%x\n", + uint32_t now = AP_HAL::millis(); + static uint32_t last_ms; + uint32_t dt = now - last_ms; + last_ms = now; + hal.console->printf("dt=%u %u RPM:%.1f THR:%.1f:%.1f V:%.2f C:%.1f CP:%.1f\n", dt, unsigned(decoded.counter), decoded.rpm, - unsigned(decoded.throttle_req), unsigned(decoded.throttle), - decoded.voltage, - decoded.phase_current, - decoded.current, - unsigned(decoded.temperature), - unsigned(decoded.status)); + decoded.throttle_req, decoded.throttle, + decoded.voltage, decoded.current, decoded.phase_current); #endif - DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,CurrP,Curr,Temp,Status", - "QIfHHfffHH", + DataFlash_Class::instance()->Log_Write("HESC", "TimeUS,CNT,RPM,ThrR,Thr,Volt,Curr,CurrP,TempM,TempC,Status", + "QIffffffBBH", AP_HAL::micros64(), decoded.counter, decoded.rpm, - decoded.throttle_req, decoded.throttle, + decoded.throttle_req, + decoded.throttle, decoded.voltage, - decoded.phase_current, decoded.current, - decoded.temperature, + decoded.phase_current, + decoded.mos_temperature, + decoded.cap_temperature, decoded.status); } @@ -167,6 +199,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t chan) voltage[0] = decoded.voltage * 1000; current[0] = decoded.current; rpm[0] = decoded.rpm; + temperature[0] = MAX(decoded.mos_temperature, decoded.cap_temperature); sem->give(); mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count); } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 6fc6a2d137..5a7e83cfb6 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -42,7 +42,8 @@ class AP_ESC_Telem { uint16_t voltage; int16_t current; int16_t phase_current; - uint16_t temperature; + uint8_t mos_temperature; + uint8_t cap_temperature; uint16_t status; uint16_t crc; } pkt; @@ -53,13 +54,14 @@ class AP_ESC_Telem { struct { uint32_t counter; - uint16_t throttle_req; - uint16_t throttle; + float throttle_req; + float throttle; float rpm; float voltage; float current; float phase_current; - uint16_t temperature; + uint8_t mos_temperature; + uint8_t cap_temperature; uint16_t status; } decoded; @@ -68,4 +70,5 @@ class AP_ESC_Telem { AP_HAL::Semaphore *sem; void parse_packet(void); + uint8_t temperature_decode(uint8_t temp_raw) const; }; From 55429c6456a2743855415e585dc35400e28194ab Mon Sep 17 00:00:00 2001 From: Akshay Bajaj Date: Fri, 17 Apr 2020 13:03:35 -0700 Subject: [PATCH 10/10] DataFlash: DISABLE persist logging for 60s after disarm or arming failure --- libraries/DataFlash/DataFlash.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 3649b9e42e..9ff914447e 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -1014,7 +1014,7 @@ bool DataFlash_Class::log_while_disarmed(void) const if (_params.log_disarmed != 0) { return true; } - +/* uint32_t now = AP_HAL::millis(); uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U; @@ -1027,6 +1027,6 @@ bool DataFlash_Class::log_while_disarmed(void) const if (_last_arming_failure_ms && now - _last_arming_failure_ms < persist_ms) { return true; } - +*/ return false; }