From e03e0261a1a0c82f545e66a1e3795956c886db71 Mon Sep 17 00:00:00 2001 From: zhangteng0526 <49060609+zhangteng0526@users.noreply.github.com> Date: Wed, 3 Jul 2024 10:58:40 +0800 Subject: [PATCH 01/20] Fix buffer overflow in mavlink_receive.cpp --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 35e2879c6db7..f8e12d51cc6d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1818,7 +1818,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) if (shell) { // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message - if (serial_control_mavlink.count > 0) { + if (serial_control_mavlink.count > 0 && serial_control_mavlink.count <= sizeof(serial_control_mavlink.data)) { shell->setTargetID(msg->sysid, msg->compid); shell->write(serial_control_mavlink.data, serial_control_mavlink.count); } From c2ae6a7e24c70e5d3989343127a7b14839f71a01 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 3 Jul 2024 14:47:16 +0200 Subject: [PATCH 02/20] BatteryStatus: remove current_filtered_a Signed-off-by: Silvan Fuhrer --- msg/BatteryStatus.msg | 1 - src/drivers/batt_smbus/batt_smbus.cpp | 1 - .../cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp | 1 - src/drivers/rc/crsf_rc/CrsfRc.cpp | 2 +- src/drivers/rc_input/crsf_telemetry.cpp | 2 +- src/drivers/rc_input/ghst_telemetry.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 1 - src/drivers/tattu_can/TattuCan.cpp | 1 - src/drivers/uavcan/sensors/battery.cpp | 1 - src/lib/battery/int_res_est_replay.py | 13 ++++--------- src/lib/drivers/smbus_sbs/SBS.hpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/streams/BATTERY_STATUS.hpp | 2 +- src/modules/mavlink/streams/SYS_STATUS.hpp | 2 +- 14 files changed, 9 insertions(+), 22 deletions(-) diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index b13f721a5aa1..8b22326d38d8 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -3,7 +3,6 @@ bool connected # Whether or not a battery is connected, based on a voltage th float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown -float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 913bbbe7b82a..7be94866d064 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -122,7 +122,6 @@ void BATT_SMBUS::RunImpl() ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult; - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 4a4d316ee09c..09c306848eb5 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -77,7 +77,6 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber battery_status_s bat_status {0}; bat_status.timestamp = hrt_absolute_time(); bat_status.voltage_filtered_v = bat_info.voltage; - bat_status.current_filtered_a = bat_info.current; bat_status.current_average_a = bat_info.average_power_10sec; bat_status.remaining = bat_info.state_of_charge_pct / 100.0f; bat_status.scale = -1; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 119e24b07ca4..930b7f138414 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -225,7 +225,7 @@ void CrsfRc::Run() if (_battery_status_sub.update(&battery_status)) { uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; this->SendTelemetryBattery(voltage, current, fuel, remaining); diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 720437c82301..aa4c5ef85d0b 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -82,7 +82,7 @@ bool CRSFTelemetry::send_battery() } uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining); diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp index 6edd0de598e6..910a519a502f 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -91,7 +91,7 @@ bool GHSTTelemetry::send_battery_status() if (_battery_status_sub.update(&battery_status)) { voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV; - current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA; + current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA; fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH; success = ghst_send_telemetry_battery_status(_uart_fd, static_cast(voltage_in_10mV), diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index 8cfe3031ca43..6c6c484948d8 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -152,7 +152,6 @@ void Batmon::RunImpl() ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); diff --git a/src/drivers/tattu_can/TattuCan.cpp b/src/drivers/tattu_can/TattuCan.cpp index 746a24137225..ed1603c86921 100644 --- a/src/drivers/tattu_can/TattuCan.cpp +++ b/src/drivers/tattu_can/TattuCan.cpp @@ -117,7 +117,6 @@ void TattuCan::Run() battery_status.voltage_v = static_cast(tattu_message.voltage) / 1000.0f; battery_status.voltage_filtered_v = static_cast(tattu_message.voltage) / 1000.0f; battery_status.current_a = static_cast(tattu_message.current) / 1000.0f; - battery_status.current_filtered_a = static_cast(tattu_message.current) / 1000.0f; battery_status.remaining = static_cast(tattu_message.remaining_percent) / 100.0f; battery_status.temperature = static_cast(tattu_message.temperature); battery_status.capacity = tattu_message.standard_capacity; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index ab09c327407c..eecaec889667 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -106,7 +106,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure::populate_smbus_data(battery_status_s &data) ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f); - data.current_filtered_a = data.current_a; // Read remaining capacity. ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f8e12d51cc6d..cdd73df693cb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1772,7 +1772,6 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) battery_status.voltage_v = voltage_sum; battery_status.voltage_filtered_v = voltage_sum; battery_status.current_a = (float)(battery_mavlink.current_battery) / 100.0f; - battery_status.current_filtered_a = battery_status.current_a; battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; battery_status.discharged_mah = (float)battery_mavlink.current_consumed; battery_status.cell_count = cell_count; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 73068f0de360..0ab03d6d950d 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -74,7 +74,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream bat_msg.type = MAV_BATTERY_TYPE_LIPO; bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; bat_msg.energy_consumed = -1; - bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; + bat_msg.current_battery = (battery_status.connected) ? battery_status.current_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : -1; // MAVLink extension: 0 is unsupported, in uORB it's NAN bat_msg.time_remaining = (battery_status.connected && (PX4_ISFINITE(battery_status.time_remaining_s))) ? diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 9a2b2cb691ec..fc775a34d65b 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -167,7 +167,7 @@ class MavlinkStreamSysStatus : public MavlinkStream if (lowest_battery.connected) { msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; - msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.current_battery = lowest_battery.current_a * 100.0f; msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); } else { From 33701aa3d593283182dee92fa989aabedc9cdda2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 3 Jul 2024 15:00:02 +0200 Subject: [PATCH 03/20] BatteryStatus: remove voltage_filtered_a Signed-off-by: Silvan Fuhrer --- boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp | 1 - msg/BatteryStatus.msg | 1 - src/drivers/batt_smbus/batt_smbus.cpp | 1 - src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp | 1 - src/drivers/osd/atxxxx/atxxxx.cpp | 4 ++-- src/drivers/osd/atxxxx/atxxxx.h | 2 +- src/drivers/rc/crsf_rc/CrsfRc.cpp | 2 +- src/drivers/rc_input/crsf_telemetry.cpp | 2 +- src/drivers/rc_input/ghst_telemetry.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 1 - src/drivers/tattu_can/TattuCan.cpp | 1 - src/drivers/uavcan/sensors/battery.cpp | 1 - src/lib/drivers/smbus_sbs/SBS.hpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 3 --- src/modules/mavlink/streams/BATTERY_STATUS.hpp | 4 ++-- src/modules/mavlink/streams/SYS_STATUS.hpp | 2 +- 16 files changed, 9 insertions(+), 20 deletions(-) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index b26f56508216..380ebc04d004 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg) hil_battery_status.timestamp = gyro_accel_time; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 8b22326d38d8..d0beda6dbc9d 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -1,7 +1,6 @@ uint64 timestamp # time since system start (microseconds) bool connected # Whether or not a battery is connected, based on a voltage threshold float32 voltage_v # Battery voltage in volts, 0 if unknown -float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 7be94866d064..194b88fa94c0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -116,7 +116,6 @@ void BATT_SMBUS::RunImpl() // Convert millivolts to volts. new_report.voltage_v = ((float)result) / 1000.0f; - new_report.voltage_filtered_v = new_report.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 09c306848eb5..718a70844fda 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -76,7 +76,6 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber battery_status_s bat_status {0}; bat_status.timestamp = hrt_absolute_time(); - bat_status.voltage_filtered_v = bat_info.voltage; bat_status.current_average_a = bat_info.average_power_10sec; bat_status.remaining = bat_info.state_of_charge_pct / 100.0f; bat_status.scale = -1; diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index b504b943c79a..a5acabd3919d 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y) int ret = PX4_OK; // TODO: show battery symbol based on battery fill level - snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v); + snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v); buf[sizeof(buf) - 1] = '\0'; for (int i = 0; buf[i] != '\0'; i++) { @@ -330,7 +330,7 @@ OSDatxxxx::update_topics() _battery_sub.copy(&battery); if (battery.connected) { - _battery_voltage_filtered_v = battery.voltage_filtered_v; + _battery_voltage_v = battery.voltage_v; _battery_discharge_mah = battery.discharged_mah; _battery_valid = true; diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index e821e65deb32..10685eec9b98 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -111,7 +111,7 @@ class OSDatxxxx : public device::SPI, public ModuleParams, public I2CSPIDriverread_word(BATT_SMBUS_CURRENT, result); diff --git a/src/drivers/tattu_can/TattuCan.cpp b/src/drivers/tattu_can/TattuCan.cpp index ed1603c86921..1f414c090796 100644 --- a/src/drivers/tattu_can/TattuCan.cpp +++ b/src/drivers/tattu_can/TattuCan.cpp @@ -115,7 +115,6 @@ void TattuCan::Run() battery_status.state_of_health = static_cast(tattu_message.health_status); battery_status.voltage_v = static_cast(tattu_message.voltage) / 1000.0f; - battery_status.voltage_filtered_v = static_cast(tattu_message.voltage) / 1000.0f; battery_status.current_a = static_cast(tattu_message.current) / 1000.0f; battery_status.remaining = static_cast(tattu_message.remaining_percent) / 100.0f; battery_status.temperature = static_cast(tattu_message.temperature); diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index eecaec889667..77698cff16d1 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -104,7 +104,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure::populate_smbus_data(battery_status_s &data) // Convert millivolts to volts. data.voltage_v = ((float)result) * 0.001f; - data.voltage_filtered_v = data.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cdd73df693cb..06fb97c8a65f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1770,7 +1770,6 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) } battery_status.voltage_v = voltage_sum; - battery_status.voltage_filtered_v = voltage_sum; battery_status.current_a = (float)(battery_mavlink.current_battery) / 100.0f; battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; battery_status.discharged_mah = (float)battery_mavlink.current_consumed; @@ -2372,7 +2371,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; @@ -2726,7 +2724,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { battery_status_s hil_battery_status{}; hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 0ab03d6d950d..c3d15054ac2f 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -161,10 +161,10 @@ class MavlinkStreamBatteryStatus : public MavlinkStream // If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining // voltage for the subsequent field. // This won't work for voltages of more than 655 volts. - const int num_fields_required = static_cast(battery_status.voltage_filtered_v * 1000.f) / (UINT16_MAX - 1) + 1; + const int num_fields_required = static_cast(battery_status.voltage_v * 1000.f) / (UINT16_MAX - 1) + 1; if (num_fields_required <= mavlink_cell_slots) { - float remaining_voltage = battery_status.voltage_filtered_v * 1000.f; + float remaining_voltage = battery_status.voltage_v * 1000.f; for (int i = 0; i < num_fields_required - 1; ++i) { bat_msg.voltages[i] = UINT16_MAX - 1; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index fc775a34d65b..272947b1ad77 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -166,7 +166,7 @@ class MavlinkStreamSysStatus : public MavlinkStream const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; if (lowest_battery.connected) { - msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; + msg.voltage_battery = lowest_battery.voltage_v * 1000.0f; msg.current_battery = lowest_battery.current_a * 100.0f; msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); From 522a25a410c47f38a4493f8a0ed6534437e51e71 Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Wed, 12 Jun 2024 14:41:08 +0200 Subject: [PATCH 04/20] gnss(septentrio): first batch of bugfixes after internal testing Internal testing revealed usability issues. Those and some other problems are fixed. --- src/drivers/gnss/septentrio/module.h | 2 +- src/drivers/gnss/septentrio/sbf/messages.h | 4 +- src/drivers/gnss/septentrio/septentrio.cpp | 493 ++++++++++++--------- src/drivers/gnss/septentrio/septentrio.h | 66 ++- 4 files changed, 332 insertions(+), 233 deletions(-) diff --git a/src/drivers/gnss/septentrio/module.h b/src/drivers/gnss/septentrio/module.h index 28b52240b643..c935c1263e44 100644 --- a/src/drivers/gnss/septentrio/module.h +++ b/src/drivers/gnss/septentrio/module.h @@ -56,7 +56,7 @@ #endif #ifdef SEP_LOG_ERROR -#define SEP_ERR(...) {PX4_WARN(__VA_ARGS__);} +#define SEP_ERR(...) {PX4_ERR(__VA_ARGS__);} #else #define SEP_ERR(...) {} #endif diff --git a/src/drivers/gnss/septentrio/sbf/messages.h b/src/drivers/gnss/septentrio/sbf/messages.h index e51a6f469b31..487a40054c64 100644 --- a/src/drivers/gnss/septentrio/sbf/messages.h +++ b/src/drivers/gnss/septentrio/sbf/messages.h @@ -60,8 +60,8 @@ constexpr uint32_t k_dnu_u4_value {4294967295}; constexpr uint32_t k_dnu_u4_bitfield {0}; constexpr uint16_t k_dnu_u2_value {65535}; constexpr uint16_t k_dnu_u2_bitfield {0}; -constexpr float k_dnu_f4_value {-2 * 10000000000}; -constexpr double k_dnu_f8_value {-2 * 10000000000}; +constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f}; +constexpr double k_dnu_f8_value {-2.0 * 10000000000.0}; /// Maximum size of all expected messages. /// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages. diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index ad99ed6342c8..838caea2947c 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -86,6 +87,11 @@ constexpr int k_max_receiver_read_timeout = 50; */ constexpr size_t k_min_receiver_read_bytes = 32; +/** + * The baud rate of Septentrio receivers with factory default configuration. +*/ +constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200; + constexpr uint8_t k_max_command_size = 120; constexpr uint16_t k_timeout_5hz = 500; constexpr uint32_t k_read_buffer_size = 150; @@ -134,11 +140,14 @@ constexpr const char *k_frequency_25_0hz = "msec40"; constexpr const char *k_frequency_50_0hz = "msec20"; px4::atomic SeptentrioDriver::_secondary_instance {nullptr}; +uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000}; +uint32_t SeptentrioDriver::k_default_baud_rate {230400}; +orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr}; SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) : Device(MODULE_NAME), _instance(instance), - _baud_rate(baud_rate) + _chosen_baud_rate(baud_rate) { strncpy(_port, device_path, sizeof(_port) - 1); // Enforce null termination. @@ -171,6 +180,10 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u get_parameter("SEP_STREAM_LOG", &receiver_stream_log); _receiver_stream_log = receiver_stream_log; + if (_receiver_stream_log == _receiver_stream_main) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream"); + } + int32_t automatic_configuration {true}; get_parameter("SEP_AUTO_CONFIG", &automatic_configuration); _automatic_configuration = static_cast(automatic_configuration); @@ -240,15 +253,13 @@ int SeptentrioDriver::print_status() break; } - PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate); + PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate()); PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate()); PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate()); PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled"); - if (_message_gps_state.timestamp != 0) { - + if (first_gps_uorb_message_created() && _state == State::ReceivingData) { PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast(rtcm_injection_frequency())); - print_message(ORB_ID(sensor_gps), _message_gps_state); } @@ -267,7 +278,7 @@ void SeptentrioDriver::run() _uart.setPort(_port); if (_uart.open()) { - _state = State::ConfiguringDevice; + _state = State::DetectingBaudRate; } else { // Failed to open port, so wait a bit before trying again. @@ -277,14 +288,42 @@ void SeptentrioDriver::run() break; } + case State::DetectingBaudRate: { + static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000}; + expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate; + + if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) { + if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) { + _state = State::ConfiguringDevice; + + } else { + SEP_ERR("Setting local baud rate failed"); + } + + } else { + _current_baud_rate_index++; + + if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) { + _current_baud_rate_index = 0; + } + } + + break; + } + case State::ConfiguringDevice: { - if (configure() == PX4_OK) { - SEP_INFO("Automatic configuration successful"); + ConfigureResult result = configure(); + + if (!(static_cast(result) & static_cast(ConfigureResult::FailedCompletely))) { + if (static_cast(result) & static_cast(ConfigureResult::NoLogging)) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging"); + } + + SEP_INFO("Automatic configuration finished"); _state = State::ReceivingData; } else { - // Failed to configure device, so wait a bit before trying again. - px4_usleep(200000); + _state = State::DetectingBaudRate; } break; @@ -296,7 +335,7 @@ void SeptentrioDriver::run() receive_result = receive(k_timeout_5hz); if (receive_result == -1) { - _state = State::ConfiguringDevice; + _state = State::DetectingBaudRate; } if (_message_satellite_info && (receive_result & 2)) { @@ -385,19 +424,51 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[]) SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance) { - ModuleArguments arguments{}; - SeptentrioDriver *gps{nullptr}; + ModuleArguments arguments {}; + SeptentrioDriver *gps {nullptr}; if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) { return nullptr; } + if (arguments.device_path_main && arguments.device_path_secondary + && arguments.device_path_main == arguments.device_path_secondary) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different"); + return nullptr; + } + + bool valid_chosen_baud_rate {false}; + + for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) { + switch (instance) { + case Instance::Main: + if (arguments.baud_rate_main == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + + case Instance::Secondary: + if (arguments.baud_rate_secondary == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + } + } + + if (!valid_chosen_baud_rate) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu", + instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate); + } + if (instance == Instance::Main) { if (Serial::validatePort(arguments.device_path_main)) { - gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main); + gps = new SeptentrioDriver(arguments.device_path_main, instance, + valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate); } else { - PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : ""); + PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : ""); } if (gps && arguments.device_path_secondary) { @@ -410,7 +481,8 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance } else { if (Serial::validatePort(arguments.device_path_secondary)) { - gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary); + gps = new SeptentrioDriver(arguments.device_path_secondary, instance, + valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate); } else { PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : ""); @@ -425,6 +497,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance int SeptentrioDriver::custom_command(int argc, char *argv[]) { bool handled = false; + const char *failure_reason {"unknown command"}; SeptentrioDriver *driver_instance; if (!is_running()) { @@ -448,7 +521,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[]) type = ReceiverResetType::Cold; } else { - print_usage("incorrect reset type"); + failure_reason = "unknown reset type"; } if (type != ReceiverResetType::None) { @@ -457,11 +530,11 @@ int SeptentrioDriver::custom_command(int argc, char *argv[]) } } else { - print_usage("incorrect usage of reset command"); + failure_reason = "incorrect usage of reset command"; } } - return (handled) ? 0 : print_usage("unknown command"); + return handled ? 0 : print_usage(failure_reason); } int SeptentrioDriver::print_usage(const char *reason) @@ -473,25 +546,28 @@ int SeptentrioDriver::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB. - -The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on -the second uORB topic instance. It can be used for logging and heading computation. +Driver for Septentrio GNSS receivers. It can automatically configure them and make their output available for the rest of the system. +A secondary receiver is supported for redundancy, logging and dual-receiver heading. +Septentrio receiver baud rates from 57600 to 1500000 are supported. If others are used, the driver will use 230400 and give a warning. ### Examples -Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4) +Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400: +$ septentrio start -d /dev/ttyS0 -b 230400 + +Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`, +detect baud rate automatically and preserve them: $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4 -Initiate warm restart of GPS device +Perform warm reset of the receivers: $ gps reset warm )DESCR_STR"); PRINT_MODULE_USAGE_NAME("septentrio", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Primary Septentrio receiver", false); - PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true); - PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Secondary Septentrio receiver", true); - PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Primary receiver port", false); + PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true); + PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Secondary receiver port", true); + PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver"); @@ -508,15 +584,15 @@ int SeptentrioDriver::reset(ReceiverResetType type) switch (type) { case ReceiverResetType::Hot: - res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout); + res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast); break; case ReceiverResetType::Warm: - res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout); + res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast); break; case ReceiverResetType::Cold: - res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout); + res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast); break; default: @@ -553,13 +629,13 @@ int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArgument switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) { - PX4_ERR("baud rate parsing failed"); + PX4_ERR("Baud rate parsing failed"); return PX4_ERROR; } break; case 'g': if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) { - PX4_ERR("baud rate parsing failed"); + PX4_ERR("Baud rate parsing failed"); return PX4_ERROR; } break; @@ -637,42 +713,31 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type) } } -uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) { - // So we can restore the port to its original state. - const uint32_t original_baud_rate = _uart.getBaudrate(); - - // Baud rates we expect the receiver to be running at. - uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000}; - if (_baud_rate != 0) { - expected_baud_rates[0] = _baud_rate; +bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) { + if (set_baudrate(baud_rate) != PX4_OK) { + return false; } - for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) { - if (set_baudrate(expected_baud_rates[i]) != PX4_OK) { - set_baudrate(original_baud_rate); - return 0; - } + if (forced_input) { + force_input(); + } - if (forced_input) { - force_input(); - } + // Make sure that any weird data is "flushed" in the receiver. + (void)send_message("\n"); - if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) { - SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]); - set_baudrate(original_baud_rate); - return expected_baud_rates[i]; - } + if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_INFO("Detected baud rate: %lu", baud_rate); + return true; } - set_baudrate(original_baud_rate); - return 0; + return false; } int SeptentrioDriver::detect_serial_port(char* const port_name) { // Read buffer to get the COM port char buf[k_read_buffer_size]; size_t buffer_offset = 0; // The offset into the string where the next data should be read to. - hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout; + hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast; bool response_detected = false; // Receiver prints prompt after a message. @@ -682,7 +747,7 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) { do { // Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string. - int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout); + int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast); if (read_result < 0) { SEP_WARN("SBF read error"); @@ -734,132 +799,81 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) { } } -int SeptentrioDriver::configure() +SeptentrioDriver::ConfigureResult SeptentrioDriver::configure() { char msg[k_max_command_size] {}; - - // Passively detect receiver baud rate. - uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true); - - if (detected_receiver_baud_rate == 0) { - SEP_INFO("CONFIG: failed baud detection"); - return PX4_ERROR; - } - - // Set same baud rate on our end. - if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) { - SEP_INFO("CONFIG: failed local baud rate setting"); - return PX4_ERROR; - } - char com_port[5] {}; + ConfigureResult result {ConfigureResult::OK}; // Passively detect receiver port. if (detect_serial_port(com_port) != PX4_OK) { - SEP_INFO("CONFIG: failed port detection"); - return PX4_ERROR; + SEP_WARN("CONFIG: failed port detection"); + return ConfigureResult::FailedCompletely; } // We should definitely match baud rates and detect used port, but don't do other configuration if not requested. // This will force input on the receiver. That shouldn't be a problem as it's on our own connection. if (!_automatic_configuration) { - return PX4_OK; + return ConfigureResult::OK; } // If user requested specific baud rate, set it now. Otherwise keep detected baud rate. - if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) { - snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate); + if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) { + snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate); if (!send_message(msg)) { - SEP_INFO("CONFIG: baud rate command write error"); - return PX4_ERROR; + SEP_WARN("CONFIG: baud rate command write error"); + return ConfigureResult::FailedCompletely; } // When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate. // From testing this could take some time. - px4_usleep(1000000); + px4_usleep(2000000); - if (set_baudrate(_baud_rate) != PX4_OK) { - SEP_INFO("CONFIG: failed local baud rate setting"); - return PX4_ERROR; + if (set_baudrate(_chosen_baud_rate) != PX4_OK) { + SEP_WARN("CONFIG: failed local baud rate setting"); + return ConfigureResult::FailedCompletely; } - if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) { - SEP_INFO("CONFIG: failed remote baud rate setting"); - return PX4_ERROR; + if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed remote baud rate setting"); + return ConfigureResult::FailedCompletely; } - } else { - _baud_rate = detected_receiver_baud_rate; } // Delete all sbf outputs on current COM port to remove clutter data snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_INFO("CONFIG: failed delete output"); - return PX4_ERROR; // connection and/or baudrate detection failed - } - - // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor - snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF"); - - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - return PX4_ERROR; - } - - // Specify the offsets that the receiver applies to the computed attitude angles. - snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset); - - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - return PX4_ERROR; - } - - snprintf(msg, sizeof(msg), k_command_set_dynamics, "high"); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - return PX4_ERROR; + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed delete output"); + return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed } - const char *sbf_frequency {k_frequency_10_0hz}; - switch (_sbf_output_frequency) { - case SBFOutputFrequency::Hz5_0: - sbf_frequency = k_frequency_5_0hz; - break; - case SBFOutputFrequency::Hz10_0: - sbf_frequency = k_frequency_10_0hz; - break; - case SBFOutputFrequency::Hz20_0: - sbf_frequency = k_frequency_20_0hz; - break; - case SBFOutputFrequency::Hz25_0: - sbf_frequency = k_frequency_25_0hz; - break; - } - - // Output a set of SBF blocks on a given connection at a regular interval. - snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_INFO("Failed to configure SBF"); - return PX4_ERROR; - } - - if (_receiver_setup == ReceiverSetup::MovingBase) { - if (_instance == Instance::Secondary) { - snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3"); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_INFO("Failed to configure RTCM output"); - } - } else { - snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_INFO("Failed to configure attitude source"); - } + // Set up the satellites used in PVT computation. + if (_receiver_constellation_usage != static_cast(SatelliteUsage::Default)) { + char requested_satellites[40] {}; + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GPS)) { + strcat(requested_satellites, "GPS+"); } - } else { - snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna); - // This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement. - if (!send_message(msg)) { - SEP_INFO("Failed to configure attitude source"); - return PX4_ERROR; + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GLONASS)) { + strcat(requested_satellites, "GLONASS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::Galileo)) { + strcat(requested_satellites, "GALILEO+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::SBAS)) { + strcat(requested_satellites, "SBAS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::BeiDou)) { + strcat(requested_satellites, "BEIDOU+"); + } + // Make sure to remove the trailing '+' if any. + requested_satellites[math::max(static_cast(strlen(requested_satellites)) - 1, 0)] = '\0'; + snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites); + // Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers. + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) { + SEP_WARN("CONFIG: Failed to configure constellation usage"); + return ConfigureResult::FailedCompletely; } } @@ -919,42 +933,77 @@ int SeptentrioDriver::configure() } snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_ERR("Failed to configure logging"); - return PX4_ERROR; + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); } } else if (_receiver_stream_log == _receiver_stream_main) { - SEP_WARN("Skipping logging setup: logging stream can't equal main stream"); + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); } - // Set up the satellites used in PVT computation. - if (_receiver_constellation_usage != static_cast(SatelliteUsage::Default)) { - char requested_satellites[40] {}; - if (_receiver_constellation_usage & static_cast(SatelliteUsage::GPS)) { - strcat(requested_satellites, "GPS+"); - } - if (_receiver_constellation_usage & static_cast(SatelliteUsage::GLONASS)) { - strcat(requested_satellites, "GLONASS+"); - } - if (_receiver_constellation_usage & static_cast(SatelliteUsage::Galileo)) { - strcat(requested_satellites, "GALILEO+"); - } - if (_receiver_constellation_usage & static_cast(SatelliteUsage::SBAS)) { - strcat(requested_satellites, "SBAS+"); - } - if (_receiver_constellation_usage & static_cast(SatelliteUsage::BeiDou)) { - strcat(requested_satellites, "BEIDOU+"); + // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF"); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + // Specify the offsets that the receiver applies to the computed attitude angles. + snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + snprintf(msg, sizeof(msg), k_command_set_dynamics, "high"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + const char *sbf_frequency {k_frequency_10_0hz}; + switch (_sbf_output_frequency) { + case SBFOutputFrequency::Hz5_0: + sbf_frequency = k_frequency_5_0hz; + break; + case SBFOutputFrequency::Hz10_0: + sbf_frequency = k_frequency_10_0hz; + break; + case SBFOutputFrequency::Hz20_0: + sbf_frequency = k_frequency_20_0hz; + break; + case SBFOutputFrequency::Hz25_0: + sbf_frequency = k_frequency_25_0hz; + break; + } + + // Output a set of SBF blocks on a given connection at a regular interval. + snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure SBF"); + return ConfigureResult::FailedCompletely; + } + + if (_receiver_setup == ReceiverSetup::MovingBase) { + if (_instance == Instance::Secondary) { + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure RTCM output"); + } + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure attitude source"); + } } - // Make sure to remove the trailing '+' if any. - requested_satellites[math::max(static_cast(strlen(requested_satellites)) - 1, 0)] = '\0'; - snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites); - if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) { - SEP_ERR("Failed to configure constellation usage"); - return PX4_ERROR; + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna); + // This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement. + if (!send_message(msg)) { + SEP_WARN("CONFIG: Failed to configure attitude source"); + return ConfigureResult::FailedCompletely; } } - return PX4_OK; + return result; } int SeptentrioDriver::parse_char(const uint8_t byte) @@ -1035,36 +1084,37 @@ int SeptentrioDriver::process_message() PVTGeodetic pvt_geodetic; if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) { - if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) { + switch (pvt_geodetic.mode_type) { + case ModeType::NoPVT: _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; - - } else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) { + break; + case ModeType::PVTWithSBAS: _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL; - - } else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) { + break; + case ModeType::RTKFloat: + case ModeType::MovingBaseRTKFloat: _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT; - - } else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) { + break; + case ModeType::RTKFixed: + case ModeType::MovingBaseRTKFixed: _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED; - - } else { + break; + default: _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D; + break; } - // Check fix and error code - _message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None; - // Check boundaries and invalidate GPS velocities if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) { _message_gps_state.vel_ned_valid = false; } - // Check boundaries and invalidate position - // We're not just checking for the do-not-use value but for any value beyond the specified max values - if (pvt_geodetic.latitude <= k_dnu_f8_value || - pvt_geodetic.longitude <= k_dnu_f8_value || - pvt_geodetic.height <= k_dnu_f8_value || - pvt_geodetic.undulation <= k_dnu_f4_value) { + if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) { + _message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG; + _message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG; + _message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast(pvt_geodetic.undulation); + _message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height; + } else { _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; } @@ -1082,23 +1132,22 @@ int SeptentrioDriver::process_message() _message_gps_state.satellites_used = 0; } - _message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG; - _message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG; - _message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast(pvt_geodetic.undulation); - _message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height; - /* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS. * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */ _message_gps_state.eph = static_cast(pvt_geodetic.h_accuracy) / 200.0f; _message_gps_state.epv = static_cast(pvt_geodetic.v_accuracy) / 200.0f; + // Check fix and error code + _message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None; _message_gps_state.vel_n_m_s = pvt_geodetic.vn; _message_gps_state.vel_e_m_s = pvt_geodetic.ve; _message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu; _message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s + _message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s); - _message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F; + if (pvt_geodetic.cog > k_dnu_f4_value) { + _message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F; + } _message_gps_state.c_variance_rad = M_DEG_TO_RAD_F; _message_gps_state.time_utc_usec = 0; @@ -1178,14 +1227,8 @@ int SeptentrioDriver::process_message() VelCovGeodetic vel_cov_geodetic; if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) { - _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve; - - if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) { - _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn; - } - - if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) { - _message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu; + if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) { + _message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu); } } @@ -1206,7 +1249,8 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&att_euler) && !att_euler.error_not_requested && att_euler.error_aux1 == Error::None && - att_euler.error_aux2 == Error::None) { + att_euler.error_aux2 == Error::None && + att_euler.heading > k_dnu_f4_value) { float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI]. // Ensure range is in [-PI, PI]. @@ -1230,7 +1274,8 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK && !att_cov_euler.error_not_requested && att_cov_euler.error_aux1 == Error::None && - att_cov_euler.error_aux2 == Error::None) { + att_cov_euler.error_aux2 == Error::None && + att_cov_euler.cov_headhead > k_dnu_f4_value) { _message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI] } @@ -1289,13 +1334,16 @@ bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int return true; } else if (expected_response[response_check_character] == buf[i]) { ++response_check_character; + } else if (buf[i] == '$') { + // Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`) + response_check_character = 1; } else { response_check_character = 0; } } } while (timeout_time > hrt_absolute_time()); - PX4_DEBUG("Response: timeout"); + SEP_WARN("Response: timeout"); return false; } @@ -1523,6 +1571,11 @@ void SeptentrioDriver::publish_satellite_info() } } +bool SeptentrioDriver::first_gps_uorb_message_created() const +{ + return _message_gps_state.timestamp != 0; +} + void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len) { gps_inject_data_s gps_inject_data{}; @@ -1668,13 +1721,11 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time) bool SeptentrioDriver::is_healthy() const { - if (_state == State::ReceivingData) { - if (!receiver_configuration_healthy()) { - return false; - } + if (_state == State::ReceivingData && receiver_configuration_healthy()) { + return true; } - return true; + return false; } void SeptentrioDriver::reset_gps_state_message() diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index 371c0f2d98cc..ec203cd86186 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -271,9 +272,20 @@ class SeptentrioDriver : public ModuleBase, public device::Dev * @return `PX4_OK` on success, `PX4_ERROR` otherwise */ int force_input(); + + /** + * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them. + */ + static uint32_t k_supported_baud_rates[]; + + /** + * Default baud rate, used when the user requested an invalid baud rate. + */ + static uint32_t k_default_baud_rate; private: enum class State { OpeningSerialPort, + DetectingBaudRate, ConfiguringDevice, ReceivingData, }; @@ -295,9 +307,24 @@ class SeptentrioDriver : public ModuleBase, public device::Dev }; /** - * Maximum timeout to wait for command acknowledgement by the receiver. + * The result of trying to configure the receiver. + */ + enum class ConfigureResult : int32_t { + OK = 0, + FailedCompletely = 1 << 0, + NoLogging = 1 << 1, + }; + + /** + * Maximum timeout to wait for fast command acknowledgement by the receiver. */ - static constexpr uint16_t k_receiver_ack_timeout = 200; + static constexpr uint16_t k_receiver_ack_timeout_fast = 200; + + /** + * Maximum timeout to wait for slow command acknowledgement by the receiver. + * Might be the case for commands that send more output back as reply. + */ + static constexpr uint16_t k_receiver_ack_timeout_slow = 400; /** * Duration of one update monitoring interval in us. @@ -306,6 +333,11 @@ class SeptentrioDriver : public ModuleBase, public device::Dev */ static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s; + /** + * uORB type to send messages to ground control stations. + */ + static orb_advert_t k_mavlink_log_pub; + /** * The default stream for output of PVT data. */ @@ -347,13 +379,15 @@ class SeptentrioDriver : public ModuleBase, public device::Dev void schedule_reset(ReceiverResetType type); /** - * @brief Detect the current baud rate used by the receiver on the connected port. + * @brief Detect whether the receiver is running at the given baud rate. + * Does not preserve local baud rate! * - * @param force_input Choose whether the receiver forces input on the port + * @param baud_rate The baud rate to check. + * @param force_input Choose whether the receiver forces input on the port. * - * @return The detected baud rate on success, or `0` on error + * @return `true` if running at the baud rate, or `false` on error. */ - uint32_t detect_receiver_baud_rate(bool forced_input); + bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input); /** * @brief Try to detect the serial port used on the receiver side. @@ -369,9 +403,9 @@ class SeptentrioDriver : public ModuleBase, public device::Dev * * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...). * - * @return `PX4_OK` on success, `PX4_ERROR` otherwise. + * @return `ConfigureResult::OK` if configured, or error. */ - int configure(); + ConfigureResult configure(); /** * @brief Parse the next byte of a received message from the receiver. @@ -505,6 +539,13 @@ class SeptentrioDriver : public ModuleBase, public device::Dev */ void publish_satellite_info(); + /** + * @brief Check whether the driver has created its first complete `SensorGPS` uORB message. + * + * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting. + */ + bool first_gps_uorb_message_created() const; + /** * @brief Publish RTCM corrections. * @@ -579,6 +620,9 @@ class SeptentrioDriver : public ModuleBase, public device::Dev /** * @brief Check whether the current receiver configuration is likely healthy. * + * This is checked by passively monitoring output from the receiver and validating whether it is what is + * expected. + * * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured. */ bool receiver_configuration_healthy() const; @@ -611,6 +655,9 @@ class SeptentrioDriver : public ModuleBase, public device::Dev /** * @brief Check whether the driver is operating correctly. * + * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the + * expected data. + * * @return `true` if the driver is working as expected, `false` otherwise. */ bool is_healthy() const; @@ -666,7 +713,7 @@ class SeptentrioDriver : public ModuleBase, public device::Dev uint8_t _spoofing_state {0}; ///< Receiver spoofing state uint8_t _jamming_state {0}; ///< Receiver jamming state const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls - uint32_t _baud_rate {0}; + uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user static px4::atomic _secondary_instance; hrt_abstime _sleep_end {0}; ///< End time for sleeping State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep @@ -683,6 +730,7 @@ class SeptentrioDriver : public ModuleBase, public device::Dev bool _automatic_configuration {true}; ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter ReceiverSetup _receiver_setup {ReceiverSetup::Default}; ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter int32_t _receiver_constellation_usage {0}; ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter + uint8_t _current_baud_rate_index {0}; ///< Index of the current baud rate to check // Decoding and parsing DecodingStatus _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into From 7bb239637e08014568e5f43871c443e5032cfc5e Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Mon, 17 Jun 2024 11:01:08 +0200 Subject: [PATCH 05/20] gnss(septentrio): fix error on driver start with same device paths This fixes an incorrect check of the device paths during instantiation of the Septentrio driver that caused the driver to start and not print an error message. --- src/drivers/gnss/septentrio/septentrio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 838caea2947c..cc6f5b828342 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -432,7 +432,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance } if (arguments.device_path_main && arguments.device_path_secondary - && arguments.device_path_main == arguments.device_path_secondary) { + && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) { mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different"); return nullptr; } From bfbbf2ff6f51ec5e9b71110d8e74c61922ae4f4b Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Mon, 17 Jun 2024 11:15:47 +0200 Subject: [PATCH 06/20] gnss(septentrio): improve `SEP_DUMP_COMM` parameter documentation The documentation for `SEP_DUMP_COMM` was quite unclear and users had to use the user guide to find out what exactly it did. The new documentation tries to make the purpose clearer. --- src/drivers/gnss/septentrio/module.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/gnss/septentrio/module.yaml b/src/drivers/gnss/septentrio/module.yaml index 49788b7f9759..fd06252469a1 100644 --- a/src/drivers/gnss/septentrio/module.yaml +++ b/src/drivers/gnss/septentrio/module.yaml @@ -104,7 +104,8 @@ parameters: description: short: Log GPS communication data long: | - Dump raw communication data from and to the connected receiver to the log file. + Log raw communication between the driver and connected receivers. + For example, "To receiver" will log all commands and corrections sent by the driver to the receiver. type: enum default: 0 min: 0 From 49dc896d205864d58203cfd3927dcdb9e2897cda Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Tue, 18 Jun 2024 10:11:54 +0200 Subject: [PATCH 07/20] gnss(septentrio): fix broken heading Heading wasn't working because of an incorrect check during parsing. --- src/drivers/gnss/septentrio/septentrio.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index cc6f5b828342..5b8839101732 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -1246,7 +1246,7 @@ int SeptentrioDriver::process_message() AttEuler att_euler; - if (_sbf_decoder.parse(&att_euler) && + if (_sbf_decoder.parse(&att_euler) == PX4_OK && !att_euler.error_not_requested && att_euler.error_aux1 == Error::None && att_euler.error_aux2 == Error::None && @@ -1540,10 +1540,6 @@ void SeptentrioDriver::publish() _sensor_gps_pub.publish(_message_gps_state); - // Heading/yaw data can be updated at a lower rate than the other navigation data. - // The uORB message definition requires this data to be set to a NAN if no new valid data is available. - _message_gps_state.heading = NAN; - if (_message_gps_state.spoofing_state != _spoofing_state) { if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) { From e27b252433e52c52773ebeffdb39b571a7044c5a Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Tue, 18 Jun 2024 11:28:29 +0200 Subject: [PATCH 08/20] gnss(septentrio): fix incorrect heading offset configuration Heading offset was configured as radians but should be configured as degrees on Septentrio receivers. The parameter was already in degrees but the configuration logic was changing it into radians. Also allow the entire allowed range of heading offset values for Septentrio receivers. --- src/drivers/gnss/septentrio/module.yaml | 2 +- src/drivers/gnss/septentrio/septentrio.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/gnss/septentrio/module.yaml b/src/drivers/gnss/septentrio/module.yaml index fd06252469a1..1b151f3618ba 100644 --- a/src/drivers/gnss/septentrio/module.yaml +++ b/src/drivers/gnss/septentrio/module.yaml @@ -71,7 +71,7 @@ parameters: type: float decimal: 3 default: 0 - min: 0 + min: -360 max: 360 unit: deg reboot_required: true diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 5b8839101732..095e60d5c329 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -153,8 +153,6 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u // Enforce null termination. _port[sizeof(_port) - 1] = '\0'; - reset_gps_state_message(); - int32_t enable_sat_info {0}; get_parameter("SEP_SAT_INFO", &enable_sat_info); @@ -211,6 +209,8 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u } set_device_type(DRV_GPS_DEVTYPE_SBF); + + reset_gps_state_message(); } SeptentrioDriver::~SeptentrioDriver() @@ -948,7 +948,7 @@ SeptentrioDriver::ConfigureResult SeptentrioDriver::configure() } // Specify the offsets that the receiver applies to the computed attitude angles. - snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset); + snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast(_heading_offset), static_cast(_pitch_offset)); if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { return ConfigureResult::FailedCompletely; From c0663ee85c8b52065d75fb9e024e8ec5a013eba3 Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Fri, 21 Jun 2024 11:06:45 +0200 Subject: [PATCH 09/20] gnss(septentrio): fix line lenghth of module documentation --- src/drivers/gnss/septentrio/septentrio.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 095e60d5c329..5c53db6098d5 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -546,9 +546,11 @@ int SeptentrioDriver::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Driver for Septentrio GNSS receivers. It can automatically configure them and make their output available for the rest of the system. +Driver for Septentrio GNSS receivers. +It can automatically configure them and make their output available for the rest of the system. A secondary receiver is supported for redundancy, logging and dual-receiver heading. -Septentrio receiver baud rates from 57600 to 1500000 are supported. If others are used, the driver will use 230400 and give a warning. +Septentrio receiver baud rates from 57600 to 1500000 are supported. +If others are used, the driver will use 230400 and give a warning. ### Examples From c8c46788ed079216e38a3d4d421b96f28d949ab3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 27 Jun 2024 14:17:57 +0200 Subject: [PATCH 10/20] Autostart: load airframes with priority ROMFS -> SD card --- ROMFS/px4fmu_common/init.d/rcS | 26 ++++++++++++++++---------- Tools/px4airframes/rcout.py | 6 ------ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9986a3a75374..0849dbd6cd6a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -217,25 +217,31 @@ else fi unset BOARD_RC_DEFAULTS - # - # Set parameters and env variables for selected SYS_AUTOSTART. - # - set AUTOSTART_PATH etc/init.d/rc.autostart + # Load airframe configuration based on SYS_AUTOSTART parameter if ! param compare SYS_AUTOSTART 0 then - if param greater SYS_AUTOSTART 1000000 + # rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE + # Look for airframe in ROMFS + . ${R}etc/init.d/rc.autostart + + if [ ${VEHICLE_TYPE} == none ] then - # Use external startup file + # Look for airframe on SD card if [ $SDCARD_AVAILABLE = yes ] then - set AUTOSTART_PATH etc/init.d/rc.autostart_ext + . ${R}etc/init.d/rc.autostart_ext else - echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS" + echo "ERROR [init] SD card not mounted - can't load external airframe" fi fi - . ${R}$AUTOSTART_PATH + + if [ ${VEHICLE_TYPE} == none ] + then + echo "ERROR [init] No airframe file found for SYS_AUTOSTART value" + param set SYS_AUTOSTART 0 + tune_control play error + fi fi - unset AUTOSTART_PATH # Check parameter version and reset upon airframe configuration version mismatch. # Reboot required because "param reset_all" would reset all "param set" lines from airframe. diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py index 9bc8747185a2..a6bdc9e94410 100644 --- a/Tools/px4airframes/rcout.py +++ b/Tools/px4airframes/rcout.py @@ -79,12 +79,6 @@ def __init__(self, groups, board, post_start=False): result += "then\n" result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n" result += "\t. /etc/init.d/airframes/${AIRFRAME}\n" - if not post_start: - result += "else\n" - result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n" - # Reset the configuration - result += "\tparam set SYS_AUTOSTART 0\n" - result += "\ttone_alarm ${TUNE_ERR}\n" result += "fi\n" result += "unset AIRFRAME" self.output = result From d3480d13026a6243daebdb8ee19a2107bcae38dd Mon Sep 17 00:00:00 2001 From: Dmitry Ponomarev Date: Mon, 6 Feb 2023 00:55:52 +0300 Subject: [PATCH 11/20] Cyphal: add port.List --- src/drivers/cyphal/Cyphal.cpp | 43 +++++++++++++++++++++- src/drivers/cyphal/Cyphal.hpp | 3 ++ src/drivers/cyphal/PublicationManager.cpp | 9 +++++ src/drivers/cyphal/PublicationManager.hpp | 2 + src/drivers/cyphal/SubscriptionManager.cpp | 21 +++++++++++ src/drivers/cyphal/SubscriptionManager.hpp | 2 + 6 files changed, 79 insertions(+), 1 deletion(-) diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 73de1992ff51..5ce3a0b77a01 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -125,7 +125,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate) _instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD); } else { - _instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC); + _instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC); } if (_instance == nullptr) { @@ -188,6 +188,8 @@ void CyphalNode::Run() // send uavcan::node::Heartbeat_1_0 @ 1 Hz sendHeartbeat(); + sendPortList(); + // Check all publishers _pub_manager.update(); @@ -392,6 +394,45 @@ void CyphalNode::sendHeartbeat() } } +void CyphalNode::sendPortList() +{ + static hrt_abstime _uavcan_node_port_List_last{0}; + + if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) { + return; + } + + static uavcan_node_port_List_0_1 msg{}; + static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_]; + static CanardTransferID _uavcan_node_port_List_transfer_id{0}; + size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_; + const hrt_abstime now = hrt_absolute_time(); + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _uavcan_node_port_List_transfer_id++ + }; + + // memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_); + uavcan_node_port_List_0_1_initialize_(&msg); + + _pub_manager.fillSubjectIdList(msg.publishers); + _sub_manager.fillSubjectIdList(msg.subscribers); + + uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size); + + _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &uavcan_node_port_List_0_1_buffer + ); + + _uavcan_node_port_List_last = now; +} + bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 004a509f4039..0132830fb6c9 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -137,6 +137,9 @@ class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem // Sends a heartbeat at 1s intervals void sendHeartbeat(); + // Sends a port.List at 3s intervals + void sendPortList(); + px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver bool _initialized{false}; ///< number of actuators currently available diff --git a/src/drivers/cyphal/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp index 74ef3699c4d7..84133e037240 100644 --- a/src/drivers/cyphal/PublicationManager.cpp +++ b/src/drivers/cyphal/PublicationManager.cpp @@ -125,6 +125,15 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name) return NULL; } +void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list) +{ + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list); + + for (auto &dynpub : _dynpublishers) { + publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id(); + publishers_list.sparse_list.count++; + } +} void PublicationManager::update() { diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index 2c3da87d9d58..c996e797e9aa 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -79,6 +79,7 @@ #include #include +#include #include "Actuators/EscClient.hpp" #include "Publishers/udral/Readiness.hpp" @@ -103,6 +104,7 @@ class PublicationManager UavcanPublisher *getPublisher(const char *subject_name); + void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list); private: void updateDynamicPublications(); diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp index e489357fc2f5..ef4ae73f2bd9 100644 --- a/src/drivers/cyphal/SubscriptionManager.cpp +++ b/src/drivers/cyphal/SubscriptionManager.cpp @@ -158,3 +158,24 @@ void SubscriptionManager::updateParams() // Check for any newly-enabled subscriptions updateDynamicSubscriptions(); } + +void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list) +{ + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list); + + UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; + + auto &sparse_list = subscribers_list.sparse_list; + + while (dynsub != nullptr) { + int32_t instance_idx = 0; + + while (dynsub->isValidPortId(dynsub->id(instance_idx))) { + sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx); + sparse_list.count++; + instance_idx++; + } + + dynsub = dynsub->next(); + } +} diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index bb56e1e05776..ce5b12a54d57 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -71,6 +71,7 @@ #include #include +#include #include "Subscribers/DynamicPortSubscriber.hpp" #include "CanardInterface.hpp" @@ -100,6 +101,7 @@ class SubscriptionManager void subscribe(); void printInfo(); void updateParams(); + void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list); private: void updateDynamicSubscriptions(); From b063202b456a346e56b7ff45ba8a12a9f1a6ae61 Mon Sep 17 00:00:00 2001 From: Dmitry Ponomarev Date: Tue, 13 Dec 2022 14:46:41 +0300 Subject: [PATCH 12/20] Cyphal: remove setpoint scaling to 8192 --- src/drivers/cyphal/Actuators/EscClient.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 8d3f75fba632..c546c5b8b475 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -105,7 +105,7 @@ class UavcanEscController : public UavcanPublisher for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { if (i < num_outputs) { - msg_sp.value[i] = static_cast(outputs[i]); + msg_sp.value[i] = static_cast(outputs[i] / 8192.0); } else { // "unset" values published as NaN From 52476633a86693551da7f81b6cfa6b0b82e2fd69 Mon Sep 17 00:00:00 2001 From: Dmitry Ponomarev Date: Fri, 14 Apr 2023 17:47:10 +0300 Subject: [PATCH 13/20] Cyphal: use actual time instead of transfer id in uptime field of heartbeat --- src/drivers/cyphal/Cyphal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 5ce3a0b77a01..94030971b54e 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -361,10 +361,10 @@ void CyphalNode::sendHeartbeat() if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { uavcan_node_Heartbeat_1_0 heartbeat{}; - heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime + const hrt_abstime now = hrt_absolute_time(); + heartbeat.uptime = now / 1000000; heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; - const hrt_abstime now = hrt_absolute_time(); size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; const CanardTransferMetadata transfer_metadata = { From 515543b1c5a3c1d11053c57f7f9eca803c94de78 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 24 Apr 2023 00:11:39 +0300 Subject: [PATCH 14/20] Cyphal: divide EscClient into 2 publishers, so setpoint and readiness are 2 different ports now --- src/drivers/cyphal/Actuators/EscClient.hpp | 142 +++++++++++---------- src/drivers/cyphal/ParamManager.hpp | 3 +- src/drivers/cyphal/PublicationManager.hpp | 10 +- src/drivers/cyphal/parameters.c | 9 ++ 4 files changed, 98 insertions(+), 66 deletions(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index c546c5b8b475..27cb715caed2 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -63,16 +63,14 @@ using std::isfinite; #include #include -/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status -class UavcanEscController : public UavcanPublisher +class ReadinessPublisher : public UavcanPublisher { public: - static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : - UavcanPublisher(handle, pmgr, "udral.", "esc") { }; + ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral.", "readiness") { }; - ~UavcanEscController() {}; + ~ReadinessPublisher() {}; void update() override { @@ -95,58 +93,18 @@ class UavcanEscController : public UavcanPublisher if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) { publish_readiness(); } - }; + } - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) - { - if (_port_id > 0) { - reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; - size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - - for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { - if (i < num_outputs) { - msg_sp.value[i] = static_cast(outputs[i] / 8192.0); - - } else { - // "unset" values published as NaN - msg_sp.value[i] = NAN; - } - } - - uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - const CanardTransferMetadata transfer_metadata = { - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = _port_id, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _transfer_id, - }; - - int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, - &payload_size); + static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000; + hrt_abstime _previous_pub_time = 0; - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - &transfer_metadata, - payload_size, - &esc_sp_payload_buffer); - } - } - }; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + actuator_armed_s _armed {}; - /** - * Sets the number of rotors - */ - void set_rotor_count(uint8_t count) { _rotor_count = count; } + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + uint64_t _actuator_test_timestamp{0}; -private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const CanardRxTransfer &msg); + CanardTransferID _arming_transfer_id; void publish_readiness() { @@ -156,7 +114,7 @@ class UavcanEscController : public UavcanPublisher size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; // Only publish if we have a valid publication ID set - if (_port_id == 0) { + if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { return; } @@ -174,7 +132,7 @@ class UavcanEscController : public UavcanPublisher uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); + CanardPortID arming_pid = static_cast(static_cast(_port_id)); const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, @@ -195,17 +153,73 @@ class UavcanEscController : public UavcanPublisher &arming_payload_buffer); } }; +}; - uint8_t _rotor_count {0}; +/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status +class UavcanEscController : public UavcanPublisher +{ +public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000; - hrt_abstime _previous_pub_time = 0; + UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral.", "esc") { }; - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; - actuator_armed_s _armed {}; + ~UavcanEscController() {}; - uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; - uint64_t _actuator_test_timestamp{0}; + void update() override + { + }; - CanardTransferID _arming_transfer_id; + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) + { + if (_port_id > 0 && _port_id != CANARD_PORT_ID_UNSET) { + reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; + size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + + for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { + if (i < num_outputs) { + msg_sp.value[i] = static_cast(outputs[i] / 8192.0); + + } else { + // "unset" values published as NaN + msg_sp.value[i] = NAN; + } + } + + uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _transfer_id, + }; + + int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &esc_sp_payload_buffer); + } + } + }; + + /** + * Sets the number of rotors + */ + void set_rotor_count(uint8_t count) { _rotor_count = count; } + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const CanardRxTransfer &msg); + + uint8_t _rotor_count {0}; }; diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 67c5dbc20bf6..ef7c6ba50425 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -116,8 +116,9 @@ class UavcanParamManager private: - const UavcanParamBinder _uavcan_params[13] { + const UavcanParamBinder _uavcan_params[14] { {"uavcan.pub.udral.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.udral.readiness.0.id", "UCAN1_READ_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.actuator_outputs.0.id", "UCAN1_ACTR_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index c996e797e9aa..8defb8d8f479 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -67,7 +67,7 @@ /* Preprocessor calculation of publisher count */ #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \ - CONFIG_CYPHAL_ESC_CONTROLLER + \ + 2 * CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_READINESS_PUBLISHER + \ CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER @@ -133,6 +133,14 @@ class PublicationManager "udral.esc", 0 }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new ReadinessPublisher(handle, pmgr); + }, + "udral.readiness", + 0 + }, #endif #if CONFIG_CYPHAL_READINESS_PUBLISHER { diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index becd33fc426a..2cb06ddc2e60 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -162,6 +162,15 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1); */ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); +/** + * Cyphal ESC readiness port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1); + /** * Cyphal GPS publication port ID. * From 41bd6c92e24bc2c58c55d9c85cd5a6a4121a44be Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 24 Apr 2023 01:14:28 +0300 Subject: [PATCH 15/20] Cyphal: add zubax.telega.CompactFeedback --- src/drivers/cyphal/Actuators/EscClient.hpp | 65 ++++++++++++++++++++++ src/drivers/cyphal/ParamManager.hpp | 3 +- src/drivers/cyphal/SubscriptionManager.hpp | 16 ++++++ src/drivers/cyphal/parameters.c | 9 +++ 4 files changed, 92 insertions(+), 1 deletion(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 27cb715caed2..c5a0c41d5b16 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -56,13 +56,17 @@ #include #include #include +#include "../Subscribers/DynamicPortSubscriber.hpp" +#include "../Publishers/Publisher.hpp" #include +#include // UDRAL Specification Messages using std::isfinite; #include #include + class ReadinessPublisher : public UavcanPublisher { public: @@ -223,3 +227,64 @@ class UavcanEscController : public UavcanPublisher uint8_t _rotor_count {0}; }; + +class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {} + + void subscribe() override + { + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + }; + + void callback(const CanardRxTransfer &receive) override + { + const ZubaxCompactFeedback* feedback = ((const ZubaxCompactFeedback*)(receive.payload)); + uint8_t esc_index = 0; + float voltage = 0.2 * feedback->dc_voltage; + float current = 0.2 * feedback->dc_current; + int32_t velocity = 0.10472 * feedback->velocity; + + mavlink_log_info(&_mavlink_log_pub, "zubax.feedback size is %d", receive.payload_size); + + if (esc_index < esc_status_s::CONNECTED_ESC_MAX) { + auto &ref = _esc_status.esc[esc_index]; + + ref.timestamp = hrt_absolute_time(); + ref.esc_address = receive.metadata.remote_node_id; + ref.esc_voltage = voltage; + ref.esc_current = current; + ref.esc_temperature = NAN; + ref.esc_rpm = velocity; + ref.esc_errorcount = 0; + + _esc_status.esc_count = 4; // _rotor_count; + _esc_status.counter += 1; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_online_flags = 7; // check_escs_status(); + _esc_status.esc_armed_flags = 7 ; // (1 << _rotor_count) - 1; + _esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(_esc_status); + } + }; + +private: + // https://telega.zubax.com/interfaces/cyphal.html#compact + struct ZubaxCompactFeedback { + uint32_t dc_voltage : 11; + int32_t dc_current : 12; + int32_t phase_current_amplitude : 12; + int32_t velocity : 13; + int8_t demand_factor_pct : 8; + }; + + esc_status_s _esc_status{}; + uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; + orb_advert_t _mavlink_log_pub{nullptr}; +}; diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index ef7c6ba50425..3eab0818bc33 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -116,7 +116,7 @@ class UavcanParamManager private: - const UavcanParamBinder _uavcan_params[14] { + const UavcanParamBinder _uavcan_params[15] { {"uavcan.pub.udral.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.readiness.0.id", "UCAN1_READ_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, @@ -131,6 +131,7 @@ class UavcanParamManager {"uavcan.sub.udral.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.0.id", "UCAN1_FB_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index ce5b12a54d57..eb9609aa5e3f 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -45,6 +45,10 @@ #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0 #endif +#ifndef CONFIG_CYPHAL_ESC_CONTROLLER +#define CONFIG_CYPHAL_ESC_CONTROLLER 0 +#endif + #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0 #endif @@ -65,6 +69,7 @@ #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ + CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ CONFIG_CYPHAL_BMS_SUBSCRIBER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER @@ -72,6 +77,7 @@ #include #include #include +#include "Actuators/EscClient.hpp" #include "Subscribers/DynamicPortSubscriber.hpp" #include "CanardInterface.hpp" @@ -132,6 +138,16 @@ class SubscriptionManager 0 }, #endif +#if CONFIG_CYPHAL_ESC_CONTROLLER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 0); + }, + "zubax.feedback", + 0 + }, +#endif #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 { [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index 2cb06ddc2e60..7564b5b3ca63 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -171,6 +171,15 @@ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); */ PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1); +/** + * Cyphal ESC zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB_SUB, -1); + /** * Cyphal GPS publication port ID. * From f81e36a3a092136fc7ca4f664702ed3a08933232 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 24 Apr 2023 08:17:40 +0300 Subject: [PATCH 16/20] Cyphal: optimize ESC setpoint --- src/drivers/cyphal/Actuators/EscClient.hpp | 73 +++++++++++----------- 1 file changed, 35 insertions(+), 38 deletions(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index c5a0c41d5b16..f4d1d81b7d4c 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -166,53 +166,48 @@ class UavcanEscController : public UavcanPublisher static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : - UavcanPublisher(handle, pmgr, "udral.", "esc") { }; + UavcanPublisher(handle, pmgr, "udral.", "esc") { } - ~UavcanEscController() {}; + ~UavcanEscController() {} void update() override { - }; + } void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { - if (_port_id > 0 && _port_id != CANARD_PORT_ID_UNSET) { - reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; - size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - - for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { - if (i < num_outputs) { - msg_sp.value[i] = static_cast(outputs[i] / 8192.0); - - } else { - // "unset" values published as NaN - msg_sp.value[i] = NAN; - } - } + if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { + return; + } + + uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS; - uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - const CanardTransferMetadata transfer_metadata = { - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = _port_id, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _transfer_id, - }; - - int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, - &payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - &transfer_metadata, - payload_size, - &esc_sp_payload_buffer); + for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) { + if (outputs[i] != 0) { + _max_number_of_nonzero_outputs = i + 1; + break; } } - }; + + uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_]; + for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) { + payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0); + } + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + }; + + ++_transfer_id; + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + _max_number_of_nonzero_outputs * 2, + &payload_buffer); + } /** * Sets the number of rotors @@ -225,7 +220,9 @@ class UavcanEscController : public UavcanPublisher */ void esc_status_sub_cb(const CanardRxTransfer &msg); + uint8_t _max_number_of_nonzero_outputs{1}; uint8_t _rotor_count {0}; + orb_advert_t _mavlink_log_pub{nullptr}; }; class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber @@ -268,7 +265,7 @@ class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; _esc_status.esc_online_flags = 7; // check_escs_status(); - _esc_status.esc_armed_flags = 7 ; // (1 << _rotor_count) - 1; + _esc_status.esc_armed_flags = 7; // (1 << _rotor_count) - 1; _esc_status.timestamp = hrt_absolute_time(); _esc_status_pub.publish(_esc_status); } From 8569eeb90c918ca3e7f4bd269035ec05538bf586 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Mon, 24 Apr 2023 18:44:32 +0300 Subject: [PATCH 17/20] Cyphal: add *type registers for ESC --- src/drivers/cyphal/ParamManager.cpp | 33 ++++++++++++++++++++++++----- src/drivers/cyphal/ParamManager.hpp | 10 +++++++++ 2 files changed, 38 insertions(+), 5 deletions(-) diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp index 15a57f3744ed..c792101ad46c 100644 --- a/src/drivers/cyphal/ParamManager.cpp +++ b/src/drivers/cyphal/ParamManager.cpp @@ -56,6 +56,15 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ } } + for (auto ¶m : _type_registers) { + if (strcmp(param_name, param.name) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } @@ -73,19 +82,33 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua } } + for (auto ¶m : _type_registers) { + if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name) { - if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder); + size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister); + if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + name.name.count = strlen(_uavcan_params[id].uavcan_name); + } else if (id < number_of_integer_registers + number_of_type_registers) { + id -= number_of_integer_registers; + strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name)); + name.name.count = strlen(_type_registers[id].name); + } else { return false; } - strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); - - name.name.count = strlen(_uavcan_params[id].uavcan_name); - return true; } diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 3eab0818bc33..056be7378506 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -103,6 +103,10 @@ typedef struct { bool is_persistent {true}; } UavcanParamBinder; +typedef struct { + const char *name; + const char *value; +} CyphalTypeRegister; class UavcanParamManager { @@ -135,4 +139,10 @@ class UavcanParamManager //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; + + CyphalTypeRegister _type_registers[3] { + {"uavcan.pub.udral.esc.0.type", "reg.udral.service.actuator.common.sp.Vector31"}, + {"uavcan.pub.udral.readiness.0.type", "reg.udral.service.common.Readiness.0.1"}, + {"uavcan.sub.zubax.feedback.0.type", "zubax.telega.CompactFeedback.0.1"}, + }; }; From 4c5ce7af6b3c1a53c419b6c5d7686d6df87f34d1 Mon Sep 17 00:00:00 2001 From: PonomarevDA Date: Thu, 4 May 2023 00:53:05 +0300 Subject: [PATCH 18/20] Cyphal: add feedback for 8 ESC --- src/drivers/cyphal/Actuators/EscClient.hpp | 109 +++++++++++---------- src/drivers/cyphal/Cyphal.cpp | 2 + src/drivers/cyphal/ParamManager.cpp | 3 + src/drivers/cyphal/ParamManager.hpp | 20 +++- src/drivers/cyphal/SubscriptionManager.hpp | 58 ++++++++++- src/drivers/cyphal/parameters.c | 67 ++++++++++++- 6 files changed, 199 insertions(+), 60 deletions(-) diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index f4d1d81b7d4c..8dbe2aff90ad 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -37,12 +37,11 @@ * Client-side implementation of UDRAL specification ESC service * * Publishes the following Cyphal messages: - * reg.drone.service.actuator.common.sp.Value8.0.1 - * reg.drone.service.common.Readiness.0.1 + * reg.udral.service.actuator.common.sp.Value31.0.1 + * reg.udral.service.common.Readiness.0.1 * * Subscribes to the following Cyphal messages: - * reg.drone.service.actuator.common.Feedback.0.1 - * reg.drone.service.actuator.common.Status.0.1 + * zubax.telega.CompactFeedback.0.1 * * @author Pavel Kirienko * @author Jacob Crabill @@ -51,7 +50,7 @@ #pragma once #include -#include +#include #include #include #include @@ -59,7 +58,6 @@ #include "../Subscribers/DynamicPortSubscriber.hpp" #include "../Publishers/Publisher.hpp" #include -#include // UDRAL Specification Messages using std::isfinite; @@ -117,7 +115,6 @@ class ReadinessPublisher : public UavcanPublisher size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - // Only publish if we have a valid publication ID set if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { return; } @@ -140,8 +137,8 @@ class ReadinessPublisher : public UavcanPublisher const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, - .port_id = arming_pid, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .port_id = arming_pid, + .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _arming_transfer_id, }; @@ -149,8 +146,7 @@ class ReadinessPublisher : public UavcanPublisher &payload_size); if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + ++_arming_transfer_id; result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, &transfer_metadata, payload_size, @@ -159,7 +155,6 @@ class ReadinessPublisher : public UavcanPublisher }; }; -/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status class UavcanEscController : public UavcanPublisher { public: @@ -190,6 +185,7 @@ class UavcanEscController : public UavcanPublisher } uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_]; + for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) { payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0); } @@ -209,20 +205,8 @@ class UavcanEscController : public UavcanPublisher &payload_buffer); } - /** - * Sets the number of rotors - */ - void set_rotor_count(uint8_t count) { _rotor_count = count; } - private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const CanardRxTransfer &msg); - uint8_t _max_number_of_nonzero_outputs{1}; - uint8_t _rotor_count {0}; - orb_advert_t _mavlink_log_pub{nullptr}; }; class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber @@ -235,44 +219,59 @@ class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber { _canard_handle.RxSubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id, - reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_subj_sub._canard_sub); + _esc_status.esc_armed_flags |= 1 << _instance; + _esc_status.esc_count++; + }; + + void unsubscribe() override + { + _canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id); + _esc_status.esc_armed_flags &= ~(1 << _instance); + _esc_status.esc_count--; }; void callback(const CanardRxTransfer &receive) override { - const ZubaxCompactFeedback* feedback = ((const ZubaxCompactFeedback*)(receive.payload)); - uint8_t esc_index = 0; - float voltage = 0.2 * feedback->dc_voltage; - float current = 0.2 * feedback->dc_current; - int32_t velocity = 0.10472 * feedback->velocity; - - mavlink_log_info(&_mavlink_log_pub, "zubax.feedback size is %d", receive.payload_size); - - if (esc_index < esc_status_s::CONNECTED_ESC_MAX) { - auto &ref = _esc_status.esc[esc_index]; - - ref.timestamp = hrt_absolute_time(); - ref.esc_address = receive.metadata.remote_node_id; - ref.esc_voltage = voltage; - ref.esc_current = current; - ref.esc_temperature = NAN; - ref.esc_rpm = velocity; - ref.esc_errorcount = 0; - - _esc_status.esc_count = 4; // _rotor_count; - _esc_status.counter += 1; - _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; - _esc_status.esc_online_flags = 7; // check_escs_status(); - _esc_status.esc_armed_flags = 7; // (1 << _rotor_count) - 1; - _esc_status.timestamp = hrt_absolute_time(); - _esc_status_pub.publish(_esc_status); + if (_instance >= esc_status_s::CONNECTED_ESC_MAX) { + return; + } + + auto &ref = _esc_status.esc[_instance]; + const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload)); + + ref.timestamp = hrt_absolute_time(); + ref.esc_address = receive.metadata.remote_node_id; + ref.esc_voltage = 0.2 * feedback->dc_voltage; + ref.esc_current = 0.2 * feedback->dc_current; + ref.esc_temperature = NAN; + ref.esc_rpm = feedback->velocity * RAD_PER_SEC_TO_RPM; + ref.esc_errorcount = 0; + + _esc_status.counter++; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1; + _esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(_esc_status); + + _esc_status.esc_online_flags = 0; + const hrt_abstime now = hrt_absolute_time(); + + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { + if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) { + _esc_status.esc_online_flags |= (1 << index); + } } }; private: + static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968; + static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7; + // https://telega.zubax.com/interfaces/cyphal.html#compact +#pragma pack(push, 1) struct ZubaxCompactFeedback { uint32_t dc_voltage : 11; int32_t dc_current : 12; @@ -280,8 +279,10 @@ class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber int32_t velocity : 13; int8_t demand_factor_pct : 8; }; +#pragma pack(pop) + static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES); + + static esc_status_s _esc_status; - esc_status_s _esc_status{}; - uORB::PublicationMulti _esc_status_pub{ORB_ID(esc_status)}; - orb_advert_t _mavlink_log_pub{nullptr}; + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; }; diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 94030971b54e..5a288274cf5d 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -62,6 +62,8 @@ using namespace time_literals; CyphalNode *CyphalNode::_instance; +esc_status_s UavcanEscFeedbackSubscriber::_esc_status; + CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp index c792101ad46c..0dba575cde1d 100644 --- a/src/drivers/cyphal/ParamManager.cpp +++ b/src/drivers/cyphal/ParamManager.cpp @@ -98,13 +98,16 @@ bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &nam { size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder); size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister); + if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); name.name.count = strlen(_uavcan_params[id].uavcan_name); + } else if (id < number_of_integer_registers + number_of_type_registers) { id -= number_of_integer_registers; strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name)); name.name.count = strlen(_type_registers[id].name); + } else { return false; } diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 056be7378506..e1330c4e5f31 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -120,7 +120,7 @@ class UavcanParamManager private: - const UavcanParamBinder _uavcan_params[15] { + const UavcanParamBinder _uavcan_params[22] { {"uavcan.pub.udral.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.readiness.0.id", "UCAN1_READ_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, @@ -135,14 +135,28 @@ class UavcanParamManager {"uavcan.sub.udral.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, - {"uavcan.sub.zubax.feedback.0.id", "UCAN1_FB_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.0.id", "UCAN1_FB0_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.1.id", "UCAN1_FB1_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.2.id", "UCAN1_FB2_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.3.id", "UCAN1_FB3_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.4.id", "UCAN1_FB4_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.5.id", "UCAN1_FB5_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.6.id", "UCAN1_FB6_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.7.id", "UCAN1_FB7_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; - CyphalTypeRegister _type_registers[3] { + CyphalTypeRegister _type_registers[10] { {"uavcan.pub.udral.esc.0.type", "reg.udral.service.actuator.common.sp.Vector31"}, {"uavcan.pub.udral.readiness.0.type", "reg.udral.service.common.Readiness.0.1"}, {"uavcan.sub.zubax.feedback.0.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.1.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.2.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.3.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.4.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.5.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.6.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.7.type", "zubax.telega.CompactFeedback.0.1"}, }; }; diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index eb9609aa5e3f..c9218d2f886c 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -69,7 +69,7 @@ #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ - CONFIG_CYPHAL_ESC_CONTROLLER + \ + 8 * CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ CONFIG_CYPHAL_BMS_SUBSCRIBER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER @@ -147,6 +147,62 @@ class SubscriptionManager "zubax.feedback", 0 }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 1); + }, + "zubax.feedback", + 1 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 2); + }, + "zubax.feedback", + 2 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 3); + }, + "zubax.feedback", + 3 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 4); + }, + "zubax.feedback", + 4 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 5); + }, + "zubax.feedback", + 5 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 6); + }, + "zubax.feedback", + 6 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 7); + }, + "zubax.feedback", + 7 + }, #endif #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 { diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index 7564b5b3ca63..4731f1c14e9c 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -172,13 +172,76 @@ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1); /** - * Cyphal ESC zubax feedback port ID. + * Cyphal ESC 0 zubax feedback port ID. * * @min -1 * @max 6143 * @group Cyphal */ -PARAM_DEFINE_INT32(UCAN1_FB_SUB, -1); +PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1); + +/** + * Cyphal ESC 1 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1); + +/** + * Cyphal ESC 2 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1); + +/** + * Cyphal ESC 3 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1); + +/** + * Cyphal ESC 4 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1); + +/** + * Cyphal ESC 5 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1); + +/** + * Cyphal ESC 6 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1); + +/** + * Cyphal ESC 7 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1); /** * Cyphal GPS publication port ID. From 1f33abb4e964d3e9c69bb71c0f84f456054a821b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 4 Jul 2024 11:57:26 +0200 Subject: [PATCH 19/20] battery_status.msg: remove unused fields (#22938) Signed-off-by: Silvan Fuhrer --- msg/BatteryStatus.msg | 4 ---- 1 file changed, 4 deletions(-) diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index d0beda6dbc9d..00d2efef4c5d 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -66,12 +66,8 @@ uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're ful uint8 MAX_INSTANCES = 4 -float32 average_power # The average power of the current discharge -float32 available_energy # The predicted charge or energy remaining in the battery float32 full_charge_capacity_wh # The compensated battery capacity float32 remaining_capacity_wh # The compensated battery capacity remaining -float32 design_capacity # The design capacity of the battery -uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes uint16 over_discharge_count # Number of battery overdischarge float32 nominal_voltage # Nominal voltage of the battery pack From a1f43636f314f05791c97d2915111bab89a82c5a Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Fri, 5 Jul 2024 03:17:19 +0200 Subject: [PATCH 20/20] ekf2: EV fusion in body frame (#23191) --- .../init.d/airframes/4061_atl_mantis_edu | 1 + .../init.d/airframes/4901_crazyflie21 | 1 + .../external_vision/ev_vel_control.cpp | 206 +++++++++++++----- src/modules/ekf2/EKF/ekf.h | 8 + .../EKF/python/ekf_derivation/derivation.py | 37 ++++ .../generated/compute_ev_body_vel_hx.h | 63 ++++++ .../generated/compute_ev_body_vel_hy.h | 67 ++++++ .../generated/compute_ev_body_vel_hz.h | 63 ++++++ .../ekf2/test/test_EKF_externalVision.cpp | 25 +-- 9 files changed, 393 insertions(+), 78 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 95df14d54ec0..27bd8397758c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -7,6 +7,7 @@ # # @maintainer # @board px4_fmu-v2 exclude +# @board px4_fmu-v6x exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index d16230d3583c..4833d17af23c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index e612452b762b..c338b75ba136 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -37,6 +37,9 @@ */ #include "ekf.h" +#include +#include +#include void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src) @@ -57,14 +60,16 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; // rotate measurement into correct earth frame if required - Vector3f vel{NAN, NAN, NAN}; - Matrix3f vel_cov{}; + Vector3f measurement{}; + Vector3f measurement_var{}; + + float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise)); switch (ev_sample.vel_frame) { case VelocityFrame::LOCAL_FRAME_NED: if (_control_status.flags.yaw_align) { - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; } else { continuing_conditions_passing = false; @@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common case VelocityFrame::LOCAL_FRAME_FRD: if (_control_status.flags.ev_yaw) { // using EV frame - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; } else { // rotate EV to the EKF reference frame const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); - vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; - vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose(); - - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = ev_sample.orientation_var.max(); - - for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max); - } + measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; + measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * + R_ev_to_ekf.transpose()).diag(); + minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max()); } break; - case VelocityFrame::BODY_FRAME_FRD: - vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - break; + case VelocityFrame::BODY_FRAME_FRD: { + + // currently it is assumed that the orientation of the EV frame and the body frame are the same + measurement = ev_sample.vel - vel_offset_body; + measurement_var = ev_sample.velocity_var; + break; + } default: continuing_conditions_passing = false; @@ -111,48 +113,56 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); + measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise)); } } #endif // CONFIG_EKF2_GNSS - const Vector3f measurement{vel}; - - const Vector3f measurement_var{ - math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f)) + measurement_var = Vector3f{ + math::max(measurement_var(0), minimum_variance), + math::max(measurement_var(1), minimum_variance), + math::max(measurement_var(2), minimum_variance) }; + continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite(); + + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + const Vector3f measurement_ekf_frame = _R_to_earth * measurement; + const uint64_t t = aid_src.timestamp_sample; + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement_ekf_frame, // observation + measurement_var_ekf_frame, // observation variance + _state.vel - measurement_ekf_frame, // innovation + getVelocityVariance() + measurement_var_ekf_frame, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + aid_src.timestamp_sample = t; + measurement.copyTo(aid_src.observation); + measurement_var.copyTo(aid_src.observation_variance); - const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); - - updateAidSourceStatus(aid_src, - ev_sample.time_us, // sample timestamp - measurement, // observation - measurement_var, // observation variance - _state.vel - measurement, // innovation - getVelocityVariance() + measurement_var, // innovation variance - math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate - - if (!measurement_valid) { - continuing_conditions_passing = false; + } else { + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement, // observation + measurement_var, // observation variance + _state.vel - measurement, // innovation + getVelocityVariance() + measurement_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate } + const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { - if (continuing_conditions_passing) { - if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { - if (quality_sufficient) { ECL_INFO("reset to %s", AID_SRC_NAME); _information_events.flags.reset_vel_to_vision = true; - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); } else { @@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseVelocity(aid_src); + fuseEvVelocity(aid_src, ev_sample); } else { aid_src.innovation_rejected = true; @@ -177,24 +187,27 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common // Data seems good, attempt a reset _information_events.flags.reset_vel_to_vision = true; ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); if (_control_status.flags.in_air) { _nb_ev_vel_reset_available--; } - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - //_control_status.flags.ev_vel_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopEvVelFusion(); - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + // differ warning message based on whether the starting conditions are passing + if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_vel_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + } + stopEvVelFusion(); } } @@ -211,15 +224,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common if (!isHorizontalAidingActive() || yaw_alignment_changed) { ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2)); - _information_events.flags.reset_vel_to_vision = true; - - resetVelocityTo(measurement, measurement_var); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.ev_vel = true; - } else if (fuseVelocity(aid_src)) { + } else if (fuseEvVelocity(aid_src, ev_sample)) { ECL_INFO("starting %s fusion", AID_SRC_NAME); _control_status.flags.ev_vel = true; } @@ -232,6 +243,63 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } } +bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample) +{ + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + + VectorState H; + estimator_aid_source1d_s current_aid_src; + const auto state_vector = _state.vector(); + + for (uint8_t index = 0; index <= 2; index++) { + current_aid_src.timestamp_sample = aid_src.timestamp_sample; + + if (index == 0) { + sym::ComputeEvBodyVelHx(state_vector, &H); + + } else if (index == 1) { + sym::ComputeEvBodyVelHy(state_vector, &H); + + } else { + sym::ComputeEvBodyVelHz(state_vector, &H); + } + + const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index]; + const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0); + + updateAidSourceStatus(current_aid_src, + ev_sample.time_us, // sample timestamp + aid_src.observation[index], // observation + aid_src.observation_variance[index], // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + + if (!current_aid_src.innovation_rejected) { + fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H); + + } + + aid_src.innovation[index] = current_aid_src.innovation; + aid_src.innovation_variance[index] = current_aid_src.innovation_variance; + aid_src.test_ratio[index] = current_aid_src.test_ratio; + aid_src.fused = current_aid_src.fused; + aid_src.innovation_rejected |= current_aid_src.innovation_rejected; + + if (aid_src.fused) { + aid_src.time_last_fuse = _time_delayed_us; + } + + } + + aid_src.timestamp_sample = current_aid_src.timestamp_sample; + return !aid_src.innovation_rejected; + + } else { + return fuseVelocity(aid_src); + } +} + void Ekf::stopEvVelFusion() { if (_control_status.flags.ev_vel) { @@ -239,3 +307,23 @@ void Ekf::stopEvVelFusion() _control_status.flags.ev_vel = false; } } + +void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, + const VelocityFrame &vel_frame) +{ + if (vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame); + + } else { + resetVelocityTo(measurement, measurement_var); + } + +} + +Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var) +{ + // rotate the covariance matrix into the EKF frame + const matrix::SquareMatrix R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose(); + return R_cov.diag(); +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2a23d1d283b4..f7c4ee2dcfc6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -921,6 +921,8 @@ class Ekf final : public EstimatorInterface void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame); + Vector3f rotateVarianceToEkf(const Vector3f &measurement_var); void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); @@ -928,6 +930,12 @@ class Ekf final : public EstimatorInterface void stopEvHgtFusion(); void stopEvVelFusion(); void stopEvYawFusion(); + bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample); + void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H) + { + VectorState Kfusion = P * H / innov_var; + aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation); + } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 4d8b5278dbcf..a206ea62954c 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -362,6 +362,40 @@ def compute_sideslip_h_and_k( return (H.T, K) +def predict_vel_body( + state: VState +) -> (sf.V3): + vel = state["vel"] + R_to_body = state["quat_nominal"].inverse() + return R_to_body * vel + +def compute_ev_body_vel_hx( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state) + Hx = jacobian_chain_rule(meas_pred[0], state) + return (Hx.T) + +def compute_ev_body_vel_hy( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[1] + Hy = jacobian_chain_rule(meas_pred, state) + return (Hy.T) + +def compute_ev_body_vel_hz( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[2] + Hz = jacobian_chain_rule(meas_pred, state) + return (Hz.T) + def predict_mag_body(state) -> sf.V3: mag_field_earth = state["mag_I"] mag_bias_body = state["mag_B"] @@ -697,5 +731,8 @@ def compute_gravity_z_innov_var_and_h( generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"]) generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"]) generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"]) +generate_px4_function(compute_ev_body_vel_hx, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hy, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hz, output_names=["H"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h new file mode 100644 index 000000000000..72a2adf0f67a --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hx + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHx(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(5, 0); + const Scalar _tmp1 = 2 * state(6, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0); + const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0); + const Scalar _tmp5 = 4 * state(4, 0); + const Scalar _tmp6 = 2 * state(1, 0); + const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp9 = 2 * state(0, 0); + const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0); + const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2; + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0); + _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0); + _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0); + _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0); + _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h new file mode 100644 index 000000000000..a4dd6f94d94c --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hy + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHy(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 64 + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = + -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0); + const Scalar _tmp5 = 4 * state(5, 0); + const Scalar _tmp6 = 2 * state(6, 0); + const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = + -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0); + _h(1, 0) = + -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0); + _h(2, 0) = + -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0); + _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0); + _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h new file mode 100644 index 000000000000..395bb85b4a2e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hz + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHz(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(5, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0); + const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp5 = 4 * state(6, 0); + const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0); + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = 2 * state(1, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9; + _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9; + _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0); + _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0); + _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0); + _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 734d485fbcd3..5b1439edd9f2 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment) TEST_F(EkfExternalVisionTest, velocityFrameBody) { - // GIVEN: Drone is turned 90 degrees - const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); - _sensor_simulator.simulateOrientation(quat_sim); + _ekf_wrapper.setMagFuseTypeNone(); _sensor_simulator.runSeconds(_tilt_align_time); - _ekf->set_vehicle_at_rest(false); - // Without any measurement x and y velocity variance are close - const Vector3f velVar_init = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); - - // WHEN: measurement is given in BODY-FRAME and - // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToBody(); + float yaw_var0 = _ekf->getYawVar(); const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f); const Vector3f vel_body(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityFrameToBody(); _sensor_simulator._vio.setVelocityVariance(vel_cov_body); _sensor_simulator._vio.setVelocity(vel_body); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); - // THEN: As the drone is turned 90 degrees, velocity variance - // along local y axis is expected to be bigger - const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f); - - const Vector3f vel_earth_est = _ekf->getVelocity(); - EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); - EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f); + const Vector3f vel_var = _ekf->getVelocityVariance(); + EXPECT_TRUE(yaw_var0 < _ekf->getYawVar()); + EXPECT_TRUE(vel_var(1) < vel_var(0)); } TEST_F(EkfExternalVisionTest, velocityFrameLocal)