From 835bab9115b49d90326a992917e8bd3e088e3fcd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:20:22 +0400 Subject: [PATCH 1/7] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 01d5bb242a..4de0338824 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01 +Subproject commit 4de0338824972de4d3a8e29697ea1a2d95a926c0 From ced75deebe527581930419155fb0d73f18b7a5fe Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:34:28 +0400 Subject: [PATCH 2/7] ESC scaling fix --- src/modules/uavcan/actuators/esc.cpp | 15 ++++++--------- src/modules/uavcan/actuators/esc.hpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 223d947312..b516536c75 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -93,25 +93,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) */ uavcan::equipment::esc::RawCommand msg; + static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); + if (_armed) { for (unsigned i = 0; i < num_outputs; i++) { - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely } - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + if (scaled > cmd_max) { + scaled = cmd_max; perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; - perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value } - msg.cmd.push_back(static_cast(scaled)); + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index cf09882104..0add7e0ad4 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -73,7 +73,7 @@ private: void orb_pub_timer_cb(const uavcan::TimerEvent &event); - static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; From e5a77a638a53aa9baec2ffe9d8ad96fb095b0966 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 20:56:45 +0400 Subject: [PATCH 3/7] ESC status feedback --- src/modules/uavcan/actuators/esc.cpp | 29 +++++++++++++++++++++++++--- src/modules/uavcan/actuators/esc.hpp | 15 +++++++------- 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index b516536c75..0601d9fa25 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -73,7 +73,7 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { perf_count(_perfcnt_invalid_input); return; } @@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() + if (msg.esc_index < CONNECTED_ESC_MAX) { + _esc_status.esc_count = uavcan::max(_esc_status.esc_count, msg.esc_index + 1); + _esc_status.timestamp = msg.getMonotonicTimestamp().toUSec(); + + auto &ref = _esc_status.esc[msg.esc_index]; + + ref.esc_address = msg.getSrcNodeID().get(); + + // >0 checks allow to weed out NaNs and negative values that aren't supported. + ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; + ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; + ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; + + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; + } } void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) { - // TODO publish to ORB + _esc_status.counter += 1; + _esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN; + + if (_esc_status_pub > 0) { + (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { + _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); + } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 0add7e0ad4..f4a3877e62 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -48,6 +48,8 @@ #include #include #include +#include + class UavcanEscController { @@ -74,8 +76,7 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; typedef uavcan::MethodBinder&)> @@ -84,6 +85,10 @@ private: typedef uavcan::MethodBinder TimerCbBinder; + bool _armed = false; + esc_status_s _esc_status = {}; + orb_advert_t _esc_status_pub = -1; + /* * libuavcan related things */ @@ -93,12 +98,6 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; - /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - /* * Perf counters */ From 1bf4270e3ee6f33f8adf0027c1a59f3fc0b35263 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:01:34 +0400 Subject: [PATCH 4/7] Update ORB topic 'esc_status' --- src/drivers/hott/messages.cpp | 14 +++++++------- src/drivers/mkblctrl/mkblctrl.cpp | 6 +++--- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- src/modules/uORB/topics/esc_status.h | 19 ++++++++++--------- src/modules/uavcan/actuators/esc.cpp | 14 ++++++-------- 5 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 0861325739..f1b12b0678 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer) esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - esc.esc[0].esc_temperature = msg.temperature1 - 20; - esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); + esc.esc[0].esc_temperature = static_cast(msg.temperature1) - 20.0F; + esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; + esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { @@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. - uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F); msg.main_voltage_L = (uint8_t)voltage & 0xff; msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; - uint16_t current = (uint16_t)(esc.esc[0].esc_current); + const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F); msg.current_L = (uint8_t)current & 0xff; msg.current_H = (uint8_t)(current >> 8) & 0xff; - uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); msg.rpm_L = (uint8_t)rpm & 0xff; msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3996b76a64..1055487cb9 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -600,8 +600,8 @@ MK::task_main() esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; esc.esc[i].esc_version = (uint16_t) Motor[i].Version; - esc.esc[i].esc_voltage = (uint16_t) 0; - esc.esc[i].esc_current = (uint16_t) Motor[i].Current; + esc.esc[i].esc_voltage = 0.0F; + esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; @@ -614,7 +614,7 @@ MK::task_main() esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } - esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; + esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d2845eb753..aeff4fd305 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -241,15 +241,15 @@ struct log_GPSP_s { #define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; - uint8_t esc_count; - uint8_t esc_connectiontype; - uint8_t esc_num; + uint8_t esc_count; + uint8_t esc_connectiontype; + uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; - uint16_t esc_voltage; - uint16_t esc_current; + float esc_voltage; + float esc_current; uint16_t esc_rpm; - uint16_t esc_temperature; + float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; }; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 628824efa8..b82796908e 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. + * Unsupported float fields should be assigned NaN. */ struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ @@ -89,17 +90,17 @@ struct esc_status_s { enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ struct { - uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ - uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ - uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ - uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ - float esc_setpoint; /**< setpoint of current ESC */ + uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ + float esc_current; /**< Current measured from current ESC [A] - if supported */ + float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ - uint16_t esc_state; /**< State of ESC - depend on Vendor */ - uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; }; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 0601d9fa25..249ffba07c 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -134,14 +134,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_address = msg.getSrcNodeID().get(); - // >0 checks allow to weed out NaNs and negative values that aren't supported. - ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; - ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; - ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; - - ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); - ref.esc_errorcount = msg.error_count; + ref.esc_voltage = msg.voltage; + ref.esc_current = msg.current; + ref.esc_temperature = msg.temperature; + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; } } From ecd144b8b4c334f4eb7a0e39e47065d6958fefaf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:07:14 +0400 Subject: [PATCH 5/7] Publishing esc_setpoint_raw from the UAVCAN driver --- src/modules/uavcan/actuators/esc.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 249ffba07c..08e5049477 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -109,6 +109,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) } msg.cmd.push_back(static_cast(scaled)); + + _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } } From f9856c6228396ca2fe8eb4c86e12c60df14d6b4c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 13:46:16 +0400 Subject: [PATCH 6/7] ESC status - supporting negative RPM --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- src/modules/uORB/topics/esc_status.h | 2 +- src/modules/uavcan/actuators/esc.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index aeff4fd305..d49fc0c799 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -248,7 +248,7 @@ struct log_ESC_s { uint16_t esc_version; float esc_voltage; float esc_current; - uint16_t esc_rpm; + int32_t esc_rpm; float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b82796908e..98cdad3d52 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -99,7 +99,7 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 08e5049477..1990651ef2 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -140,7 +140,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_current = msg.current; ref.esc_temperature = msg.temperature; ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); + ref.esc_rpm = msg.rpm; ref.esc_errorcount = msg.error_count; } } From 7fd3bd9af8ee150911a08e75a1e30fa4cb2b8e2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 14:01:17 +0400 Subject: [PATCH 7/7] esc_status layout optimization --- src/modules/uORB/topics/esc_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 98cdad3d52..b45c49788d 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,6 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ @@ -99,7 +100,6 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX];