From 4e280795b2a35522e3c55a977c6dff4687dbdf32 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Sun, 18 Feb 2024 12:15:33 +0000 Subject: [PATCH 01/52] AP_ESC_Telem: for RPM, log NaN instead of 0 when there are no measurements --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 4af6e8bd72dcb..837888b94df1e 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -503,9 +503,9 @@ void AP_ESC_Telem::update() if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { - float rpm = 0.0f; + float rpm = AP::logger().quiet_nanf(); get_rpm(i, rpm); - float raw_rpm = 0.0f; + float raw_rpm = AP::logger().quiet_nanf(); get_raw_rpm(i, raw_rpm); // Write ESC status messages From 5329ab8d927e98385f6f585ebcc738b0be04ae1d Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:26:24 +0000 Subject: [PATCH 02/52] AP_ESC_Telem: cleanup whitespace --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 837888b94df1e..1fa0eae3eb0ab 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -312,8 +312,8 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) const uint32_t now_us = AP_HAL::micros(); // loop through groups of 4 ESCs - const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); - const uint8_t num_idx = ESC_TELEM_MAX_ESCS/4; + const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS - 1); + const uint8_t num_idx = ESC_TELEM_MAX_ESCS / 4; for (uint8_t idx = 0; idx < num_idx; idx++) { const uint8_t i = (next_idx + idx) % num_idx; @@ -543,7 +543,7 @@ void AP_ESC_Telem::update() } } -bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) +bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) { // easy case, has the time window been crossed so it's invalid if ((now_us - instance.last_update_us) > timeout_us) { @@ -557,7 +557,7 @@ bool AP_ESC_Telem::rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend: return instance.data_valid; } -bool AP_ESC_Telem::was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance) +bool AP_ESC_Telem::was_rpm_data_ever_reported(const volatile AP_ESC_Telem_Backend::RpmData &instance) { return instance.last_update_us > 0; } From 8a576f884ed70b0e572f084a6558b267b05802a8 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:27:09 +0000 Subject: [PATCH 03/52] AP_ESC_Telem: remove redundant initialization --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 1fa0eae3eb0ab..df2210af83ac2 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -354,7 +354,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) s.voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); s.current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); s.totalcurrent[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); - float rpmf = 0.0f; + float rpmf; if (get_rpm(esc_id, rpmf)) { s.rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); } From 58d82000db7f68d0288a96308f37b286fb19ad67 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:41:03 +0000 Subject: [PATCH 04/52] AP_ESC_Telem: split logging and invalidation, deduplicate micros64() --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index df2210af83ac2..354849f38119e 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -492,12 +492,9 @@ void AP_ESC_Telem::update() { #if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); -#endif - - const uint32_t now_us = AP_HAL::micros(); + const uint64_t now_us64 = AP_HAL::micros64(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { -#if HAL_LOGGING_ENABLED // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] @@ -519,7 +516,7 @@ void AP_ESC_Telem::update() // error_rate is in percentage const struct log_Esc pkt{ LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), - time_us : AP_HAL::micros64(), + time_us : now_us64, instance : i, rpm : rpm, raw_rpm : raw_rpm, @@ -535,8 +532,12 @@ void AP_ESC_Telem::update() _last_rpm_log_us[i] = _rpm_data[i].last_update_us; } } + } #endif // HAL_LOGGING_ENABLED + const uint32_t now_us = AP_HAL::micros(); + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + // Invalidate RPM data if not received for too long if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { _rpm_data[i].data_valid = false; } From 0ede7f5075de3930dc8ff6fbbb528c8c68d7e1ec Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Tue, 20 Feb 2024 12:45:41 +0000 Subject: [PATCH 05/52] AP_ESC_Telem: replace selected repeated indexing with references --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 98 ++++++++++++++----------- 1 file changed, 54 insertions(+), 44 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 354849f38119e..a82a014dedd64 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -214,24 +214,26 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const // get an individual ESC's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].temperature_cdeg; + temp = telemdata.temperature_cdeg; return true; } // get an individual motor's temperature in centi-degrees if available, returns true on success bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { + || telemdata.stale() + || !(telemdata.types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { return false; } - temp = _telem_data[esc_index].motor_temp_cdeg; + temp = telemdata.motor_temp_cdeg; return true; } @@ -254,48 +256,52 @@ bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const // get an individual ESC's current in Ampere if available, returns true on success bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } - amps = _telem_data[esc_index].current; + amps = telemdata.current; return true; } // get an individual ESC's voltage in Volt if available, returns true on success bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } - volts = _telem_data[esc_index].voltage; + volts = telemdata.voltage; return true; } // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } - consumption_mah = _telem_data[esc_index].consumption_mah; + consumption_mah = telemdata.consumption_mah; return true; } // get an individual ESC's usage time in seconds if available, returns true on success bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const { + const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index]; if (esc_index >= ESC_TELEM_MAX_ESCS - || _telem_data[esc_index].stale() - || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { + || telemdata.stale() + || !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } - usage_s = _telem_data[esc_index].usage_s; + usage_s = telemdata.usage_s; return true; } @@ -349,16 +355,17 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) if (esc_id >= ESC_TELEM_MAX_ESCS) { continue; } + volatile AP_ESC_Telem_Backend::TelemetryData const &telemdata = _telem_data[esc_id]; - s.temperature[j] = _telem_data[esc_id].temperature_cdeg / 100; - s.voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); - s.current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); - s.totalcurrent[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); + s.temperature[j] = telemdata.temperature_cdeg / 100; + s.voltage[j] = constrain_float(telemdata.voltage * 100.0f, 0, UINT16_MAX); + s.current[j] = constrain_float(telemdata.current * 100.0f, 0, UINT16_MAX); + s.totalcurrent[j] = constrain_float(telemdata.consumption_mah, 0, UINT16_MAX); float rpmf; if (get_rpm(esc_id, rpmf)) { s.rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); } - s.count[j] = _telem_data[esc_id].count; + s.count[j] = telemdata.count; } // make sure a msg hasn't been extended @@ -425,41 +432,42 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem } _have_data = true; + volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[esc_index]; #if AP_TEMPERATURE_SENSOR_ENABLED // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) || - ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); + ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)); #else const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE); #endif if (has_temperature) { - _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; + telemdata.temperature_cdeg = new_data.temperature_cdeg; } if (has_motor_temperature) { - _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; + telemdata.motor_temp_cdeg = new_data.motor_temp_cdeg; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) { - _telem_data[esc_index].voltage = new_data.voltage; + telemdata.voltage = new_data.voltage; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) { - _telem_data[esc_index].current = new_data.current; + telemdata.current = new_data.current; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) { - _telem_data[esc_index].consumption_mah = new_data.consumption_mah; + telemdata.consumption_mah = new_data.consumption_mah; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { - _telem_data[esc_index].usage_s = new_data.usage_s; + telemdata.usage_s = new_data.usage_s; } - _telem_data[esc_index].count++; - _telem_data[esc_index].types |= data_mask; - _telem_data[esc_index].last_update_ms = AP_HAL::millis(); + telemdata.count++; + telemdata.types |= data_mask; + telemdata.last_update_ms = AP_HAL::millis(); } // record an update to the RPM together with timestamp, this allows the notch values to be slewed @@ -495,10 +503,12 @@ void AP_ESC_Telem::update() const uint64_t now_us64 = AP_HAL::micros64(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i]; + const volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i]; // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { - if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] - || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { + if (telemdata.last_update_ms != _last_telem_log_ms[i] + || rpmdata.last_update_us != _last_rpm_log_us[i]) { float rpm = AP::logger().quiet_nanf(); get_rpm(i, rpm); @@ -520,16 +530,16 @@ void AP_ESC_Telem::update() instance : i, rpm : rpm, raw_rpm : raw_rpm, - voltage : _telem_data[i].voltage, - current : _telem_data[i].current, - esc_temp : _telem_data[i].temperature_cdeg, - current_tot : _telem_data[i].consumption_mah, - motor_temp : _telem_data[i].motor_temp_cdeg, - error_rate : _rpm_data[i].error_rate + voltage : telemdata.voltage, + current : telemdata.current, + esc_temp : telemdata.temperature_cdeg, + current_tot : telemdata.consumption_mah, + motor_temp : telemdata.motor_temp_cdeg, + error_rate : rpmdata.error_rate }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); - _last_telem_log_ms[i] = _telem_data[i].last_update_ms; - _last_rpm_log_us[i] = _rpm_data[i].last_update_us; + _last_telem_log_ms[i] = telemdata.last_update_ms; + _last_rpm_log_us[i] = rpmdata.last_update_us; } } } From 957b05e53efeb58e2f1951990ccc92fe124b7136 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Tue, 13 Feb 2024 18:44:08 +0100 Subject: [PATCH 06/52] AP_PiccoloCAN: Remove duplicated code Fix doxygen markup for consistency Fix typos --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 76 ------------------- libraries/AP_PiccoloCAN/AP_PiccoloCAN.h | 15 ++-- .../AP_PiccoloCAN/AP_PiccoloCAN_Device.h | 16 ++-- libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp | 2 +- 4 files changed, 15 insertions(+), 94 deletions(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index bbaec88f3e4d0..428f589b89270 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -403,82 +403,6 @@ void AP_PiccoloCAN::update() #endif // HAL_LOGGING_ENABLED } -#if HAL_GCS_ENABLED -// send ESC telemetry messages over MAVLink -void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) -{ - // Arrays to store ESC telemetry data - uint8_t temperature[4] {}; - uint16_t voltage[4] {}; - uint16_t rpm[4] {}; - uint16_t count[4] {}; - uint16_t current[4] {}; - uint16_t totalcurrent[4] {}; - - bool dataAvailable = false; - - uint8_t idx = 0; - - WITH_SEMAPHORE(_telem_sem); - - for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) { - - // Calculate index within storage array - idx = (ii % 4); - - AP_PiccoloCAN_ESC &esc = _escs[idx]; - - // Has the ESC been heard from recently? - if (is_esc_present(ii)) { - dataAvailable = true; - - // Provide the maximum ESC temperature in the telemetry stream - temperature[idx] = esc.temperature(); // Convert to C - voltage[idx] = esc.voltage() * 10; // Convert to cV - current[idx] = esc.current() * 10; // Convert to cA - totalcurrent[idx] = 0; - rpm[idx] = esc.rpm(); - count[idx] = 0; - } else { - temperature[idx] = 0; - voltage[idx] = 0; - current[idx] = 0; - totalcurrent[idx] = 0; - rpm[idx] = 0; - count[idx] = 0; - } - - // Send ESC telemetry in groups of 4 - if ((ii % 4) == 3) { - - if (dataAvailable) { - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) { - continue; - } - - switch (ii) { - case 3: - mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 7: - mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 11: - mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - case 15: - mavlink_msg_esc_telemetry_13_to_16_send((mavlink_channel_t) mav_chan, temperature, voltage, current, totalcurrent, rpm, count); - break; - default: - break; - } - } - - dataAvailable = false; - } - } -} -#endif // send servo messages over CAN void AP_PiccoloCAN::send_servo_messages(void) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 770f05dcf53bf..3615f3db4fa29 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -56,9 +56,6 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend // called from SRV_Channels void update(); - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); - // return true if a particular servo is 'active' on the Piccolo interface bool is_servo_channel_active(uint8_t chan); @@ -125,14 +122,14 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend } _ecu_info; // Piccolo CAN parameters - AP_Int32 _esc_bm; //! ESC selection bitmask - AP_Int16 _esc_hz; //! ESC update rate (Hz) + AP_Int32 _esc_bm; //!< ESC selection bitmask + AP_Int16 _esc_hz; //!< ESC update rate (Hz) - AP_Int32 _srv_bm; //! Servo selection bitmask - AP_Int16 _srv_hz; //! Servo update rate (Hz) + AP_Int32 _srv_bm; //!< Servo selection bitmask + AP_Int16 _srv_hz; //!< Servo update rate (Hz) - AP_Int16 _ecu_id; //! ECU Node ID - AP_Int16 _ecu_hz; //! ECU update rate (Hz) + AP_Int16 _ecu_id; //!< ECU Node ID + AP_Int16 _ecu_hz; //!< ECU update rate (Hz) HAL_Semaphore _telem_sem; }; diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h index e0b3cd9f169bf..fff6e17539568 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_Device.h @@ -27,18 +27,18 @@ #if HAL_PICCOLO_CAN_ENABLE -// Piccolo message groups form part of the CAN ID of each frame +//! Piccolo message groups form part of the CAN ID of each frame enum class PiccoloCAN_MessageGroup : uint8_t { - SIMULATOR = 0x00, // Simulator messages - SENSOR = 0x04, // External sensors - ACTUATOR = 0x07, // Actuators (e.g. ESC / servo) - ECU_OUT = 0x08, // Messages *from* an ECU - ECU_IN = 0x09, // Message *to* an ECU + SIMULATOR = 0x00, //!< Simulator messages + SENSOR = 0x04, //!< External sensors + ACTUATOR = 0x07, //!< Actuators (e.g. ESC / servo) + ECU_OUT = 0x08, //!< Messages *from* an ECU + ECU_IN = 0x09, //!< Message *to* an ECU - SYSTEM = 0x19, // System messages (e.g. bootloader) + SYSTEM = 0x19, //!< System messages (e.g. bootloader) }; -// Piccolo actuator types differentiate between actuator frames +//! Piccolo actuator types differentiate between actuator frames enum class PiccoloCAN_ActuatorType : uint8_t { SERVO = 0x00, ESC = 0x20, diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp index 8f45dda7002be..24c4ed760ca7e 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN_ESC.cpp @@ -20,7 +20,7 @@ #if HAL_PICCOLO_CAN_ENABLE /* - * Decode a recevied CAN frame. + * Decode a received CAN frame. * It is assumed at this point that the received frame is intended for *this* ESC */ bool AP_PiccoloCAN_ESC::handle_can_frame(AP_HAL::CANFrame &frame) From fc1c30bc3686fee3e9c6d53f1f6380d0a2f8ca15 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 21:31:03 +1100 Subject: [PATCH 07/52] AP_MSP: correct compilation when GPS disabled ../../libraries/AP_MSP/AP_MSP_Telem_Backend.cpp: In member function 'virtual void AP_MSP_Telem_Backend::update_gps_state(AP_MSP_Telem_Backend::gps_state_t&)': ../../libraries/AP_MSP/AP_MSP_Telem_Backend.cpp:206:5: error: 'AP_GPS' was not declared in this scope; did you mean 'RAW_GPS'? 206 | AP_GPS& gps = AP::gps(); | ^~~~~~ | RAW_GPS compilation terminated due to -Wfatal-errors. --- libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index e107dffe5f4d6..9a2d09ba18847 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -201,6 +201,7 @@ void AP_MSP_Telem_Backend::update_home_pos(home_state_t &home_state) home_state.home_is_set = _ahrs.home_is_set(); } +#if AP_GPS_ENABLED void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state) { AP_GPS& gps = AP::gps(); @@ -220,6 +221,7 @@ void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state) gps_state.ground_course_cd = gps.ground_course_cd(); } } +#endif #if AP_BATTERY_ENABLED void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state) @@ -645,8 +647,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst) return MSP_RESULT_ERROR; } #endif - gps_state_t gps_state; + gps_state_t gps_state {}; +#if AP_GPS_ENABLED update_gps_state(gps_state); +#endif // handle airspeed override bool airspeed_en = false; From 591d65ec918ebb0928ef14d48f5a1c98a1c213f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 25 Feb 2024 16:56:08 +1100 Subject: [PATCH 08/52] CI: fix ESP32 CI issue ensure apt db is up to date before first install --- .github/workflows/esp32_build.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index 26185663fe7ac..91926070e1227 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -175,6 +175,7 @@ jobs: - name: Install Prerequisites shell: bash run: | + sudo apt-get update sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 update-alternatives --query python From 2a67cbe6814bfda78cd5127e2e8e5e8dfdc56ec3 Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Fri, 23 Feb 2024 22:58:53 +0000 Subject: [PATCH 09/52] AP_Logger: Add metadata for VER and FILE messages --- libraries/AP_Logger/LogStructure.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 8845c9ba00fc2..54dd754e6200d 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1195,6 +1195,13 @@ struct PACKED log_VER { // @Field: Free: free stack // @Field: Name: thread name +// @LoggerMessage: FILE +// @Description: File data +// @Field: FileName: File name +// @Field: Offset: Offset into the file of this block +// @Field: Length: Length of this data block +// @Field: Data: File data of this block + // @LoggerMessage: SCR // @Description: Scripting runtime stats // @Field: TimeUS: Time since system startup @@ -1203,6 +1210,20 @@ struct PACKED log_VER { // @Field: Total_mem: total memory usage of all scripts // @Field: Run_mem: run memory usage +// @LoggerMessage: VER +// @Description: Ardupilot version +// @Field: TimeUS: Time since system startup +// @Field: BT: Board type +// @Field: BST: Board subtype +// @Field: Maj: Major version number +// @Field: Min: Minor version number +// @Field: Pat: Patch number +// @Field: FWT: Firmware type +// @Field: GH: Github commit +// @Field: FWS: Firmware version string +// @Field: APJ: Board ID +// @Field: BU: Build vehicle type + // @LoggerMessage: MOTB // @Description: Motor mixer information // @Field: TimeUS: Time since system startup From edd03b1ac266922e02a9218d3bf0b15fc74e43bb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 26 Feb 2024 07:49:21 +1100 Subject: [PATCH 10/52] AP_Periph: release notes for 1.7.0 --- Tools/AP_Periph/ReleaseNotes.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 252044e1efceb..5119d6ee8cc05 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,18 @@ +Release 1.7.0 26th February 2023 +-------------------------------- + +This is a major AP_Periph release with the following key changes: + +- fixed DroneCAN packet parsing bug when dealing with corrupt packets +- added BATT_HIDE_MASK parameter +- support IPv4 networking in AP_Periph and PPP gateway +- support per-cell battery monitoring +- rate limit EFI updates +- support serial tunnelling over DroneCAN +- support relays over DroneCAN via hardpoint messages +- support mapping MAVLink SEND_TEXT to DroneCAN debug levels +- fixed CANFD timings + Release 1.6.0 8th September 2023 -------------------------------- From 0007c7dce0a6afa28dddd7bf26a0a17821be0aa0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 26 Feb 2024 07:53:39 +1100 Subject: [PATCH 11/52] AP_Periph: mark master as 1.8.0 dev --- Tools/AP_Periph/version.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index ed2dccddb44c4..2eb0a235a8ce5 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -7,7 +7,7 @@ #include "ap_version.h" #include -#define THISFIRMWARE "AP_Periph V1.7.0-dev" +#define THISFIRMWARE "AP_Periph V1.8.0-dev" // defines needed due to lack of GCS includes #ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE @@ -17,10 +17,10 @@ #endif // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,7,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,8,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 1 -#define FW_MINOR 7 +#define FW_MINOR 8 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV From bf1d36420108026c29c426f3d46965c30e9d786b Mon Sep 17 00:00:00 2001 From: Richard Allen Date: Sun, 11 Feb 2024 12:15:50 -0600 Subject: [PATCH 12/52] HAL_Linux: reduce delay(ms) jitter Fix delay(1) rarely returning immediately. On my RPi4, this once per 5-20k calls that worked. Reduce the last call to microsleep according to the remaining time needed in the last loop iteration. --- libraries/AP_HAL_Linux/Scheduler.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 7c2cce5d2bfb3..98e3914332274 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -173,15 +173,20 @@ void Scheduler::delay(uint16_t ms) return; } - uint64_t start = AP_HAL::millis64(); + if (ms == 0) { + return; + } - while ((AP_HAL::millis64() - start) < ms) { + uint64_t now = AP_HAL::micros64(); + uint64_t end = now + 1000UL * ms + 1U; + do { // this yields the CPU to other apps - microsleep(1000); + microsleep(MIN(1000UL, end-now)); if (in_main_thread() && _min_delay_cb_ms <= ms) { call_delay_cb(); } - } + now = AP_HAL::micros64(); + } while (now < end); } void Scheduler::delay_microseconds(uint16_t us) From 846eaecc0b488a2899f540e6569882b9df3e4038 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 19 Feb 2024 20:17:42 +1100 Subject: [PATCH 13/52] AP_TECS: Add option for speed to increase up to FBW max during descent --- libraries/AP_TECS/AP_TECS.cpp | 14 +++++++++++++- libraries/AP_TECS/AP_TECS.h | 7 ++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index dd9c8b428e68d..7ef4241798564 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -246,7 +246,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: OPTIONS // @DisplayName: Extra TECS options // @Description: This allows the enabling of special features in the speed/height controller. - // @Bitmask: 0:GliderOnly + // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup // @User: Advanced AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0), @@ -458,6 +458,11 @@ void AP_TECS::_update_speed(float DT) void AP_TECS::_update_speed_demand(void) { + if (_options & OPTION_DESCENT_SPEEDUP) { + // Allow demanded speed to go to maximum when descending at maximum descent rate + _TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction; + } + // Set the airspeed demand to the minimum value if an underspeed condition exists // or a bad descent condition exists // This will minimise the rate of descent resulting from an engine failure, @@ -532,9 +537,16 @@ void AP_TECS::_update_height_demand(void) // Limit height rate of change if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) { _hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT; + _sink_fraction = 0.0f; } else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) { _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT; + _sink_fraction = 1.0f; } else { + if (is_negative(hgt_dem - _hgt_dem_rate_ltd)) { + _sink_fraction = (hgt_dem - _hgt_dem_rate_ltd) / (-_sink_rate_limit * _DT); + } else { + _sink_fraction = 0.0f; + } _hgt_dem_rate_ltd = hgt_dem; } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index b392a0bfc555b..50b91a882c346 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -197,7 +197,8 @@ class AP_TECS { AP_Float _hgt_dem_tconst; enum { - OPTION_GLIDER_ONLY=(1<<0) + OPTION_GLIDER_ONLY=(1<<0), + OPTION_DESCENT_SPEEDUP=(1<<1) }; AP_Float _pitch_ff_v0; @@ -334,6 +335,9 @@ class AP_TECS { // true when a reset of airspeed and height states to current is performed on this frame bool reset:1; + + // true when we are allowing the plane to speed up on descent to maintain the target descent rate + bool speedup:1; }; union { struct flags _flags; @@ -389,6 +393,7 @@ class AP_TECS { // used to scale max climb and sink limits to match vehicle ability float _max_climb_scaler; float _max_sink_scaler; + float _sink_fraction; // Specific energy error quantities float _STE_error; From aecbd116e7f597b1ad993ed33aa28aa50f154cd0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 21 Feb 2024 18:56:18 +1100 Subject: [PATCH 14/52] AP_TECS: Add /0 protection --- libraries/AP_TECS/AP_TECS.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 7ef4241798564..82f1aa06cae23 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -542,8 +542,10 @@ void AP_TECS::_update_height_demand(void) _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT; _sink_fraction = 1.0f; } else { - if (is_negative(hgt_dem - _hgt_dem_rate_ltd)) { - _sink_fraction = (hgt_dem - _hgt_dem_rate_ltd) / (-_sink_rate_limit * _DT); + const float numerator = hgt_dem - _hgt_dem_rate_ltd; + const float denominator = - _sink_rate_limit * _DT; + if (is_negative(numerator) && is_negative(denominator)) { + _sink_fraction = numerator / denominator; } else { _sink_fraction = 0.0f; } From 870c6c508037b3baeb681d458c2b4b21d714298f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 21 Feb 2024 18:58:41 +1100 Subject: [PATCH 15/52] AP_TECS: Remove unused variable --- libraries/AP_TECS/AP_TECS.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 50b91a882c346..4470b700f546f 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -336,8 +336,6 @@ class AP_TECS { // true when a reset of airspeed and height states to current is performed on this frame bool reset:1; - // true when we are allowing the plane to speed up on descent to maintain the target descent rate - bool speedup:1; }; union { struct flags _flags; From 6d6c790e379004694d8210c41089cf869f55535d Mon Sep 17 00:00:00 2001 From: Andrew Piper Date: Sat, 24 Feb 2024 18:32:12 +0000 Subject: [PATCH 16/52] AP_GPS: fragments_received is a bitmask not a count --- libraries/AP_GPS/AP_GPS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 94e98f0ac1d83..9b15335435200 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1633,7 +1633,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ // we have one or more partial fragments already received // which conflict with the new fragment, discard previous fragments rtcm_buffer->fragment_count = 0; - rtcm_stats.fragments_discarded += rtcm_buffer->fragments_received; + rtcm_stats.fragments_discarded += __builtin_popcount(rtcm_buffer->fragments_received); rtcm_buffer->fragments_received = 0; } @@ -1662,7 +1662,7 @@ void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_ if (rtcm_buffer->fragment_count != 0 && rtcm_buffer->fragments_received == (1U << rtcm_buffer->fragment_count) - 1) { // we have them all, inject - rtcm_stats.fragments_used += rtcm_buffer->fragments_received; + rtcm_stats.fragments_used += __builtin_popcount(rtcm_buffer->fragments_received); inject_data(rtcm_buffer->buffer, rtcm_buffer->total_length); rtcm_buffer->fragment_count = 0; rtcm_buffer->fragments_received = 0; From df3267c9bab79637b426db02469539ca0dea93df Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 25 Feb 2024 20:09:01 +1100 Subject: [PATCH 17/52] waf: remove SKETCH and SKETCHNAME from build sysmte ... renaming one of them to AP_BUILD_TARGET --- Tools/ardupilotwaf/ap_library.py | 2 +- Tools/ardupilotwaf/ardupilotwaf.py | 6 ++---- libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 2 +- libraries/AP_HAL_ChibiOS/Storage.cpp | 4 ++-- libraries/AP_HAL_Linux/Storage.cpp | 2 +- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 +- 6 files changed, 8 insertions(+), 10 deletions(-) diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index b49010ae06001..67cb61428248b 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -52,7 +52,7 @@ def _vehicle_index(vehicle): return _vehicle_indexes[vehicle] # note that AP_NavEKF3_core.h is needed for AP_NavEKF3_feature.h -_vehicle_macros = ['SKETCHNAME', 'SKETCH', 'APM_BUILD_DIRECTORY', +_vehicle_macros = ['APM_BUILD_DIRECTORY', 'AP_BUILD_TARGET_NAME', 'APM_BUILD_TYPE', 'APM_BUILD_COPTER_OR_HELI', 'AP_NavEKF3_core.h', 'lua_generated_bindings.h'] _macros_re = re.compile(r'\b(%s)\b' % '|'.join(_vehicle_macros)) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 882e2f86ca408..490fc21d9e287 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -125,14 +125,12 @@ def get_legacy_defines(sketch_name, bld): if bld.cmd == 'heli' or 'heli' in bld.targets: return [ 'APM_BUILD_DIRECTORY=APM_BUILD_Heli', - 'SKETCH="' + sketch_name + '"', - 'SKETCHNAME="' + sketch_name + '"', + 'AP_BUILD_TARGET_NAME="' + sketch_name + '"', ] return [ 'APM_BUILD_DIRECTORY=APM_BUILD_' + sketch_name, - 'SKETCH="' + sketch_name + '"', - 'SKETCHNAME="' + sketch_name + '"', + 'AP_BUILD_TARGET_NAME="' + sketch_name + '"', ] IGNORED_AP_LIBRARIES = [ diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index eb741e1ae7c52..ea55e7140f08a 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -286,7 +286,7 @@ static void main_loop() hal.scheduler->set_system_initialized(); thread_running = true; - chRegSetThreadName(SKETCHNAME); + chRegSetThreadName(AP_BUILD_TARGET_NAME); /* switch to high priority for main loop diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index e4ee42cc4ddb4..7f8fc2819b9c8 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -31,9 +31,9 @@ using namespace ChibiOS; extern const AP_HAL::HAL& hal; #ifndef HAL_STORAGE_FILE -// using SKETCHNAME allows the one microSD to be used +// using AP_BUILD_TARGET_NAME allows the one microSD to be used // for multiple vehicle types -#define HAL_STORAGE_FILE "/APM/" SKETCHNAME ".stg" +#define HAL_STORAGE_FILE "/APM/" AP_BUILD_TARGET_NAME ".stg" #endif #ifndef HAL_STORAGE_BACKUP_FOLDER diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp index 4f9fc133a274a..4fc401c76405b 100644 --- a/libraries/AP_HAL_Linux/Storage.cpp +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -20,7 +20,7 @@ using namespace Linux; // name the storage file after the sketch so you can use the same board // card for ArduCopter and ArduPlane -#define STORAGE_FILE SKETCHNAME ".stg" +#define STORAGE_FILE AP_BUILD_TARGET_NAME ".stg" extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 96ec46d879d99..f4c5529197ff4 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -212,7 +212,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) // default to CMAC const char *home_str = nullptr; const char *model_str = nullptr; - const char *vehicle_str = SKETCH; + const char *vehicle_str = AP_BUILD_TARGET_NAME; _use_fg_view = false; char *autotest_dir = nullptr; _fg_address = "127.0.0.1"; From e9d065c1cc011ab879f31285991aa6c6e57d102c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 26 Feb 2024 14:58:10 +1100 Subject: [PATCH 18/52] AP_NavEKF3: tidy population of fusion reports simply take a refefence and use it --- .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index c8df1a5c0742f..9708b60f164ef 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -268,11 +268,12 @@ void NavEKF3_core::FuseRngBcn() // Update the fusion report if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) { - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio; + auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID]; + report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED; + report.innov = rngBcn.innov; + report.innovVar = rngBcn.varInnov; + report.rng = rngBcn.dataDelayed.rng; + report.testRatio = rngBcn.testRatio; } } } @@ -506,11 +507,12 @@ void NavEKF3_core::FuseRngBcnStatic() } // Update the fusion report if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) { - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].beaconPosNED = rngBcn.dataDelayed.beacon_posNED; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innov = rngBcn.innov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].innovVar = rngBcn.varInnov; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].rng = rngBcn.dataDelayed.rng; - rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID].testRatio = rngBcn.testRatio; + auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID]; + report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED; + report.innov = rngBcn.innov; + report.innovVar = rngBcn.varInnov; + report.rng = rngBcn.dataDelayed.rng; + report.testRatio = rngBcn.testRatio; } } } From ac769014c4b100f7afd5ff0b2971787560f31759 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 17 Oct 2022 10:25:30 +1100 Subject: [PATCH 19/52] AP_Filesystem: list virtual @SYS, @MISSION etc directories in / Adds virtual directory entries for these virtual filesystems in / RTL> ftp list RTL> Listing / D @MISSION D @PARAM D @ROMFS D @SYS D APM D log V5_BT.dfu 10541 bootlog.txt 297 dataman 350216 message-intervals-chan0.txt 7 Total size 352.60 kByte ftp list @MISSION RTL> Listing @MISSION LIST: OP seq:7 sess:2 opcode:129 req_opcode:3 size:2 bc:0 ofs:0 plen=2 [2] ftp list @ROMFS RTL> Listing @ROMFS bootloader.bin 16448 hwdef.dat 5743 io_firmware.bin 40880 Total size 61.59 kByte This PR also makes us *much* more lenient in what we accept for looking at virtual filesystems, so ftp list @SYS ftp list /@SYS ftp list @SYS/ ftp list /@SYS/ should all work --- libraries/AP_Filesystem/AP_Filesystem.cpp | 51 ++++++++++++++++++++--- libraries/AP_Filesystem/AP_Filesystem.h | 8 ++++ 2 files changed, 53 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index cd7e88a88ecf8..45c58654bf786 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -63,18 +63,16 @@ static AP_Filesystem_Mission fs_mission; const AP_Filesystem::Backend AP_Filesystem::backends[] = { { nullptr, fs_local }, #if AP_FILESYSTEM_ROMFS_ENABLED - { "@ROMFS/", fs_romfs }, { "@ROMFS", fs_romfs }, #endif #if AP_FILESYSTEM_PARAM_ENABLED - { "@PARAM/", fs_param }, + { "@PARAM", fs_param }, #endif #if AP_FILESYSTEM_SYS_ENABLED - { "@SYS/", fs_sys }, { "@SYS", fs_sys }, #endif #if AP_FILESYSTEM_MISSION_ENABLED - { "@MISSION/", fs_mission }, + { "@MISSION", fs_mission }, #endif }; @@ -90,10 +88,19 @@ extern const AP_HAL::HAL& hal; */ const AP_Filesystem::Backend &AP_Filesystem::backend_by_path(const char *&path) const { + // ignore leading slashes: + const char *path_with_no_leading_slash = path; + if (path_with_no_leading_slash[0] == '/') { + path_with_no_leading_slash = &path_with_no_leading_slash[1]; + } for (uint8_t i=1; i 0 && path[0] == '/') { + path++; + } return backends[i]; } } @@ -188,6 +195,18 @@ int AP_Filesystem::rename(const char *oldpath, const char *newpath) AP_Filesystem::DirHandle *AP_Filesystem::opendir(const char *pathname) { + // support reading a list of "@" filesystems (e.g. @SYS) in + // listing of root directory. Note that backend_by_path modifies + // its parameter. + if (strlen(pathname) == 0 || + (strlen(pathname) == 1 && pathname[0] == '/')) { + virtual_dirent.backend_ofs = 0; + virtual_dirent.d_off = 0; + virtual_dirent.de.d_type = DT_DIR; + } else { + virtual_dirent.backend_ofs = 255; + } + const Backend &backend = backend_by_path(pathname); DirHandle *h = new DirHandle; if (!h) { @@ -199,6 +218,7 @@ AP_Filesystem::DirHandle *AP_Filesystem::opendir(const char *pathname) return nullptr; } h->fs_index = BACKEND_IDX(backend); + return h; } @@ -208,7 +228,26 @@ struct dirent *AP_Filesystem::readdir(DirHandle *dirp) return nullptr; } const Backend &backend = backends[dirp->fs_index]; - return backend.fs.readdir(dirp->dir); + struct dirent * ret = backend.fs.readdir(dirp->dir); + if (ret != nullptr) { + return ret; + } + + // virtual directory entries in the root directory (e.g. @SYS, @MISSION) + for (; ret == nullptr && virtual_dirent.backend_ofs < ARRAY_SIZE(AP_Filesystem::backends); virtual_dirent.backend_ofs++) { + const char *prefix = backends[virtual_dirent.backend_ofs].prefix; + if (prefix == nullptr) { + continue; + } + if (prefix[0] != '@') { + continue; + } + // found a virtual directory we haven't returned yet + strncpy_noterm(virtual_dirent.de.d_name, prefix, sizeof(virtual_dirent.de.d_name)); + virtual_dirent.d_off++; + ret = &virtual_dirent.de; + } + return ret; } int AP_Filesystem::closedir(DirHandle *dirp) diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index 9a5e01e6933c2..a6ebc358dcdf8 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -155,6 +155,14 @@ class AP_Filesystem { find backend by open fd */ const Backend &backend_by_fd(int &fd) const; + + // support for listing out virtual directory entries (e.g. @SYS + // then @MISSION) + struct { + uint8_t backend_ofs; + struct dirent de; + uint8_t d_off; + } virtual_dirent; }; namespace AP { From b7dd432409d8358057c8d02a45413880fd3ce2d3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 23 Feb 2024 01:22:03 +0000 Subject: [PATCH 20/52] AP_Scripting: allow loading moduels from ROMFS --- libraries/AP_Scripting/lua/src/loadlib.c | 4 ++++ libraries/AP_Scripting/lua/src/luaconf.h | 4 ++++ libraries/AP_Scripting/lua_bindings.cpp | 27 ++++++++++++++++++++++++ libraries/AP_Scripting/lua_common_defs.h | 1 + 4 files changed, 36 insertions(+) diff --git a/libraries/AP_Scripting/lua/src/loadlib.c b/libraries/AP_Scripting/lua/src/loadlib.c index 9ef7edb215d5c..20af8e47660c0 100644 --- a/libraries/AP_Scripting/lua/src/loadlib.c +++ b/libraries/AP_Scripting/lua/src/loadlib.c @@ -798,7 +798,11 @@ LUAMOD_API int luaopen_package (lua_State *L) { luaL_newlib(L, pk_funcs); /* create 'package' table */ // createsearcherstable(L); /* set paths */ +#if defined(ARDUPILOT_BUILD) + setpath(L, "path", LUA_PATH_VAR, lua_get_modules_path()); +#else setpath(L, "path", LUA_PATH_VAR, LUA_PATH_DEFAULT); +#endif // setpath(L, "cpath", LUA_CPATH_VAR, LUA_CPATH_DEFAULT); /* store config information */ lua_pushliteral(L, LUA_DIRSEP "\n" LUA_PATH_SEP "\n" LUA_PATH_MARK "\n" diff --git a/libraries/AP_Scripting/lua/src/luaconf.h b/libraries/AP_Scripting/lua/src/luaconf.h index f148e1d48ff0a..885a40f14b0be 100644 --- a/libraries/AP_Scripting/lua/src/luaconf.h +++ b/libraries/AP_Scripting/lua/src/luaconf.h @@ -195,11 +195,13 @@ #define LUA_LDIR "!\\lua\\" #define LUA_CDIR "!\\" #define LUA_SHRDIR "!\\..\\share\\lua\\" LUA_VDIR "\\" +#if !defined(ARDUPILOT_BUILD) #define LUA_PATH_DEFAULT \ LUA_LDIR"?.lua;" LUA_LDIR"?\\init.lua;" \ LUA_CDIR"?.lua;" LUA_CDIR"?\\init.lua;" \ LUA_SHRDIR"?.lua;" LUA_SHRDIR"?\\init.lua;" \ ".\\?.lua;" ".\\?\\init.lua" +#endif #define LUA_CPATH_DEFAULT \ LUA_CDIR"?.dll;" \ LUA_CDIR"..\\lib\\lua\\" LUA_VDIR "\\?.dll;" \ @@ -210,9 +212,11 @@ #define LUA_ROOT "/usr/local/" #define LUA_LDIR SCRIPTING_DIRECTORY "/modules/" #define LUA_CDIR LUA_ROOT "lib/lua/" LUA_VDIR "/" +#if !defined(ARDUPILOT_BUILD) #define LUA_PATH_DEFAULT \ LUA_LDIR"?.lua;" LUA_LDIR"?/init.lua;" \ "./?.lua;" "./?/init.lua" +#endif #define LUA_CPATH_DEFAULT \ LUA_CDIR"?.so;" LUA_CDIR"loadall.so;" "./?.so" #endif /* } */ diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 6e234130eb0e1..b1f7baea56152 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -920,6 +920,33 @@ int lua_get_current_ref() return scripting->get_current_ref(); } +// This is used when loading modules with require, lua must only look in enabled directory's +const char* lua_get_modules_path() +{ +#define LUA_PATH_ROMFS "@ROMFS/scripts/modules/?.lua;" "@ROMFS/scripts/modules/?/init.lua" +#define LUA_PATH_SCRIPTS LUA_LDIR"?.lua;" LUA_LDIR"?/init.lua" + + uint16_t dir_disable = AP_Scripting::get_singleton()->get_disabled_dir(); + dir_disable &= uint16_t(AP_Scripting::SCR_DIR::SCRIPTS) | uint16_t(AP_Scripting::SCR_DIR::ROMFS); + if (dir_disable == 0) { + // Both directory's are enabled, return both, ROMFS takes priority + return LUA_PATH_ROMFS ";" LUA_PATH_SCRIPTS; + } + + if ((dir_disable & uint16_t(AP_Scripting::SCR_DIR::SCRIPTS)) == 0) { + // Only scripts enabled + return LUA_PATH_SCRIPTS; + } + + if ((dir_disable & uint16_t(AP_Scripting::SCR_DIR::ROMFS)) == 0) { + // Only ROMFS enabled + return LUA_PATH_ROMFS; + } + + // Nothing enabled? + return ""; +} + // Simple print to GCS or over CAN int lua_print(lua_State *L) { // Only support a single argument diff --git a/libraries/AP_Scripting/lua_common_defs.h b/libraries/AP_Scripting/lua_common_defs.h index 664541ddfa003..9776e687a8900 100644 --- a/libraries/AP_Scripting/lua_common_defs.h +++ b/libraries/AP_Scripting/lua_common_defs.h @@ -27,5 +27,6 @@ #endif // REPL_OUT int lua_get_current_ref(); +const char* lua_get_modules_path(); void lua_abort(void) __attribute__((noreturn)); From f2e9e8427883c2d1e5fb9997a1b18ae2b231c7a7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 22:23:06 +1100 Subject: [PATCH 21/52] AP_Param: simplify g2 object conversion --- libraries/AP_Param/AP_Param.cpp | 14 ++++++++++++++ libraries/AP_Param/AP_Param.h | 8 ++++++++ 2 files changed, 22 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 57a82684956d0..4743bd4fbd8e2 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2072,6 +2072,20 @@ void AP_Param::convert_class(uint16_t param_key, void *object_pointer, flush(); } +void AP_Param::convert_g2_objects(const void *g2, const G2ObjectConversion g2_conversions[], uint8_t num_conversions) +{ + // Find G2's Top Level Key + ConversionInfo info; + if (!find_top_level_key_by_pointer(g2, info.old_key)) { + return; + } + for (uint8_t i=0; i Date: Wed, 21 Feb 2024 22:32:35 +1100 Subject: [PATCH 22/52] ArduPlane: simplify g2 object conversion --- ArduPlane/Parameters.cpp | 47 +++++++++------------------------------- 1 file changed, 10 insertions(+), 37 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fe571b2b58293..6bc16df613c45 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1527,49 +1527,22 @@ void Plane::load_parameters(void) landing.convert_parameters(); - // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 + static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_STATS_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 5; // Old parameter index in g2 - const uint16_t old_top_element = 4037; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); - } -#endif // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 + { &stats, stats.var_info, 5, 4037 }, +#endif #if AP_SCRIPTING_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 14; // Old parameter index in g2 - const uint16_t old_top_element = 78; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 + { &scripting, scripting.var_info, 14, 78 }, #endif - - // PARAMETER_CONVERSION - Added: Feb-2024 for Plane-4.6 #if AP_GRIPPER_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 12; // Old parameter index in g2 - const uint16_t old_top_element = 4044; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &gripper, gripper.var_info, old_index, old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Feb-2024 for Plane-4.6 + { &gripper, gripper.var_info, 12, 4044 }, #endif + }; + + AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 #if HAL_LOGGING_ENABLED From 620723767d17d05191c735d916b2afae54a1443f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 22:32:35 +1100 Subject: [PATCH 23/52] ArduSub: simplify g2 object conversion --- ArduSub/Parameters.cpp | 65 +++++++++--------------------------------- 1 file changed, 14 insertions(+), 51 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 1a469c4c1e3da..a1bf37988862a 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -745,68 +745,31 @@ void Sub::load_parameters() // We should ignore this parameter since ROVs are neutral buoyancy AP_Param::set_by_name("MOT_THST_HOVER", 0.5); -// PARAMETER_CONVERSION - Added: JAN-2022 -#if AP_AIRSPEED_ENABLED - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 19; // Old parameter index in the tree - const uint16_t old_top_element = 4051; // Old group element in the tree for the first subgroup element - AP_Param::convert_class(info.old_key, &airspeed, airspeed.var_info, old_index, old_top_element, false); -#endif - - // PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); #endif - // PARAMETER_CONVERSION - Added: Jan-2024 -#if AP_STATS_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo stats_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, stats_info.old_key)) { - return; - } - - const uint16_t stats_old_index = 1; // Old parameter index in g2 - const uint16_t stats_old_top_element = 4033; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(stats_info.old_key, &stats, stats.var_info, stats_old_index, stats_old_top_element, false); - } + static const AP_Param::G2ObjectConversion g2_conversions[] { +#if AP_AIRSPEED_ENABLED + // PARAMETER_CONVERSION - Added: JAN-2022 + { &airspeed, airspeed.var_info, 19, 4051 }, #endif +#if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 + { &stats, stats.var_info, 1, 4033 }, +#endif #if AP_SCRIPTING_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo scripting_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, scripting_info.old_key)) { - return; - } - - const uint16_t scripting_old_index = 18; // Old parameter index in g2 - const uint16_t scripting_old_top_element = 82; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(scripting_info.old_key, &scripting, scripting.var_info, scripting_old_index, scripting_old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Jan-2024 + { &scripting, scripting.var_info, 18, 82 }, #endif - - // PARAMETER_CONVERSION - Added: Feb-2024 #if AP_GRIPPER_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo gripper_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, gripper_info.old_key)) { - return; - } - - const uint16_t old_gripper_index = 3; // Old parameter index in g2 - const uint16_t old_gripper_top_element = 4035; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(gripper_info.old_key, &gripper, gripper.var_info, old_gripper_index, old_gripper_top_element, false); - } + // PARAMETER_CONVERSION - Added: Feb-2024 + { &gripper, gripper.var_info, 3, 4035 }, #endif + }; + + AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); // PARAMETER_CONVERSION - Added: Feb-2024 #if HAL_LOGGING_ENABLED From 1e2fdb0ca19191bc60080fbeab374c5222d90268 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 22:44:25 +1100 Subject: [PATCH 24/52] ArduCopter: convert EFI cvonversion to g2_conversion entry --- ArduCopter/Parameters.cpp | 51 +++++++++------------------------------ 1 file changed, 12 insertions(+), 39 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index fcde858fc4b6d..cd47458393f37 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1357,53 +1357,26 @@ void Copter::load_parameters(void) AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); #endif - // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_STATS_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 12; // Old parameter index in g2 - const uint16_t old_top_element = 4044; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); - } -#endif // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + { &stats, stats.var_info, 12, 4044 }, +#endif #if AP_SCRIPTING_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 30; // Old parameter index in g2 - const uint16_t old_top_element = 94; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + { &scripting, scripting.var_info, 30, 94 }, #endif - +#if AP_GRIPPER_ENABLED // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 -#if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + { &gripper, gripper.var_info, 13, 4045}, #endif + }; - // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 -#if AP_GRIPPER_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } + AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); - const uint16_t old_index = 13; // Old parameter index in g2 - const uint16_t old_top_element = 4045; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &gripper, gripper.var_info, old_index, old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 +#if HAL_LOGGING_ENABLED + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); #endif // setup AP_Param frame type flags From 30b8905b09447ba1a937af18512c47c50fbc0599 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 23:02:36 +1100 Subject: [PATCH 25/52] ArduPlane: convert EFI cvonversion to g2_conversion entry --- ArduPlane/Parameters.cpp | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 6bc16df613c45..367745b75ad12 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1468,22 +1468,6 @@ void Plane::load_parameters(void) g.use_reverse_thrust.convert_parameter_width(AP_PARAM_INT16); - - // PARAMETER_CONVERSION - Added: Oct-2021 -#if HAL_EFI_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 22; // Old parameter index in g2 - const uint16_t old_top_element = 86; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &efi, efi.var_info, old_index, old_top_element, false); - } -#endif - #if AP_AIRSPEED_ENABLED // PARAMETER_CONVERSION - Added: Jan-2022 { @@ -1528,6 +1512,10 @@ void Plane::load_parameters(void) landing.convert_parameters(); static const AP_Param::G2ObjectConversion g2_conversions[] { + // PARAMETER_CONVERSION - Added: Oct-2021 +#if HAL_EFI_ENABLED + { &efi, efi.var_info, 22, 86 }, +#endif #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 { &stats, stats.var_info, 5, 4037 }, From a1cd6df32c0402b4befd15e8316bb6f98fa0cc49 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 23:02:36 +1100 Subject: [PATCH 26/52] Blimp: convert EFI cvonversion to g2_conversion entry --- Blimp/Parameters.cpp | 31 +++++++------------------------ 1 file changed, 7 insertions(+), 24 deletions(-) diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index cbc14fc4101a2..b13b103af4f59 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -842,34 +842,17 @@ void Blimp::load_parameters(void) { AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version); - // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_STATS_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 12; // Old parameter index in g2 - const uint16_t old_top_element = 4044; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); - } -#endif // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + { &stats, stats.var_info, 12, 4044 }, +#endif #if AP_SCRIPTING_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { - return; - } - - const uint16_t old_index = 30; // Old parameter index in g2 - const uint16_t old_top_element = 94; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 + { &scripting, scripting.var_info, 30, 94 }, #endif + }; + AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); // PARAMETER_CONVERSION - Added: Feb-2024 #if HAL_LOGGING_ENABLED From 9af383f63491da31cc1dd13b7dfba21d94ab4848 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Feb 2024 23:02:36 +1100 Subject: [PATCH 27/52] Rover: convert EFI cvonversion to g2_conversion entry --- Rover/Parameters.cpp | 64 +++++++++++--------------------------------- 1 file changed, 16 insertions(+), 48 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 7a4d0e312eb50..a2a4b0f203bf7 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -875,66 +875,34 @@ void Rover::load_parameters(void) } #endif -// PARAMETER_CONVERSION - Added: JAN-2022 + static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_AIRSPEED_ENABLED - const uint16_t old_index = 37; // Old parameter index in the tree - const uint16_t old_top_element = 4069; // Old group element in the tree for the first subgroup element - AP_Param::convert_class(info.old_key, &airspeed, airspeed.var_info, old_index, old_top_element, false); +// PARAMETER_CONVERSION - Added: JAN-2022 + { &airspeed, airspeed.var_info, 37, 4069 }, #endif - -// PARAMETER_CONVERSION - Added: MAR-2022 #if AP_AIS_ENABLED - AP_Param::convert_class(info.old_key, &ais, ais.var_info, 50, 114, false); +// PARAMETER_CONVERSION - Added: MAR-2022 + { &ais, ais.var_info, 50, 114 }, #endif - -// PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED - AP_Param::convert_class(info.old_key, &fence, fence.var_info, 17, 4049, false); +// PARAMETER_CONVERSION - Added: Mar-2022 + { &fence, fence.var_info, 17, 4049 }, #endif - - // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 #if AP_STATS_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo stats_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, stats_info.old_key)) { - return; - } - - const uint16_t stats_old_index = 1; // Old parameter index in g2 - const uint16_t stats_old_top_element = 4033; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(stats_info.old_key, &stats, stats.var_info, stats_old_index, stats_old_top_element, false); - } -#endif // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 + { &stats, stats.var_info, 1, 4033 }, +#endif #if AP_SCRIPTING_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo scripting_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, scripting_info.old_key)) { - return; - } - - const uint16_t scripting_old_index = 41; // Old parameter index in g2 - const uint16_t scripting_old_top_element = 105; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(scripting_info.old_key, &scripting, scripting.var_info, scripting_old_index, scripting_old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 + { &scripting, scripting.var_info, 41, 105 }, #endif - - // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 #if AP_GRIPPER_ENABLED - { - // Find G2's Top Level Key - AP_Param::ConversionInfo gripper_info; - if (!AP_Param::find_top_level_key_by_pointer(&g2, gripper_info.old_key)) { - return; - } - - const uint16_t gripper_old_index = 39; // Old parameter index in g2 - const uint16_t gripper_old_top_element = 4071; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(gripper_info.old_key, &gripper, gripper.var_info, gripper_old_index, gripper_old_top_element, false); - } + // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 + { &gripper, gripper.var_info, 39, 4071 }, #endif + }; + + AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); // PARAMETER_CONVERSION - Added: Feb-2024 for Rover-4.6 #if HAL_LOGGING_ENABLED From dd628b025fb4cf72f4e6a39dacc2433bb09d1a8c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:31:59 +1100 Subject: [PATCH 28/52] AP_Param: remove unused old_top_element param from convert_class --- libraries/AP_Param/AP_Param.cpp | 4 ++-- libraries/AP_Param/AP_Param.h | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 4743bd4fbd8e2..826c308a467fb 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2031,7 +2031,7 @@ void AP_Param::convert_old_parameters_scaled(const struct ConversionInfo *conver // is_top_level: Is true if the class had its own top level key, param_key. It is false if the class was a subgroup void AP_Param::convert_class(uint16_t param_key, void *object_pointer, const struct AP_Param::GroupInfo *group_info, - uint16_t old_index, uint16_t old_top_element, bool is_top_level) + uint16_t old_index, bool is_top_level) { const uint8_t group_shift = is_top_level ? 0 : 6; @@ -2081,7 +2081,7 @@ void AP_Param::convert_g2_objects(const void *g2, const G2ObjectConversion g2_co } for (uint8_t i=0; i Date: Thu, 22 Feb 2024 08:31:59 +1100 Subject: [PATCH 29/52] AntennaTracker: remove unused old_top_element param from convert_class --- AntennaTracker/Parameters.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 4eb3db11691f5..5be95dedf7dde 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -588,17 +588,17 @@ void Tracker::load_parameters(void) #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 - AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, true); #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 - AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, true); #endif // PARAMETER_CONVERSION - Added: Feb-2024 for Tracker-4.6 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif #if HAL_HAVE_SAFETY_SWITCH From 198f26f34824285d6804622a00a71f021c46fa0f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:32:00 +1100 Subject: [PATCH 30/52] ArduCopter: remove unused old_top_element param from convert_class --- ArduCopter/Parameters.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index cd47458393f37..8a1cec0c3ab00 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1354,21 +1354,21 @@ void Copter::load_parameters(void) // PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED - AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, true); #endif static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 - { &stats, stats.var_info, 12, 4044 }, + { &stats, stats.var_info, 12 }, #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 - { &scripting, scripting.var_info, 30, 94 }, + { &scripting, scripting.var_info, 30 }, #endif #if AP_GRIPPER_ENABLED // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 - { &gripper, gripper.var_info, 13, 4045}, + { &gripper, gripper.var_info, 13 }, #endif }; @@ -1376,7 +1376,7 @@ void Copter::load_parameters(void) // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif // setup AP_Param frame type flags From 7a1f3579049f2cc86b532625145b41867bd1fa5d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:32:00 +1100 Subject: [PATCH 31/52] ArduPlane: remove unused old_top_element param from convert_class --- ArduPlane/Parameters.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 367745b75ad12..b6db6ba4ae6f9 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1473,8 +1473,7 @@ void Plane::load_parameters(void) { const uint16_t old_key = g.k_param_airspeed; const uint16_t old_index = 0; // Old parameter index in the tree - const uint16_t old_top_element = 0; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) - AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, old_top_element, true); + AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, true); } #endif @@ -1495,7 +1494,7 @@ void Plane::load_parameters(void) // PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED - AP_Param::convert_class(g.k_param_fence, &fence, fence.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_fence, &fence, fence.var_info, 0, true); #endif // PARAMETER_CONVERSION - Added: Dec 2023 @@ -1514,19 +1513,19 @@ void Plane::load_parameters(void) static const AP_Param::G2ObjectConversion g2_conversions[] { // PARAMETER_CONVERSION - Added: Oct-2021 #if HAL_EFI_ENABLED - { &efi, efi.var_info, 22, 86 }, + { &efi, efi.var_info, 22 }, #endif #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 - { &stats, stats.var_info, 5, 4037 }, + { &stats, stats.var_info, 5 }, #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 - { &scripting, scripting.var_info, 14, 78 }, + { &scripting, scripting.var_info, 14 }, #endif #if AP_GRIPPER_ENABLED // PARAMETER_CONVERSION - Added: Feb-2024 for Plane-4.6 - { &gripper, gripper.var_info, 12, 4044 }, + { &gripper, gripper.var_info, 12 }, #endif }; @@ -1534,6 +1533,6 @@ void Plane::load_parameters(void) // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif } From 5e35c8850feb7366ca22b9ed5e526c8747ef87ee Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:32:00 +1100 Subject: [PATCH 32/52] ArduSub: remove unused old_top_element param from convert_class --- ArduSub/Parameters.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index a1bf37988862a..2b400abdf8fb7 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -747,25 +747,25 @@ void Sub::load_parameters() // PARAMETER_CONVERSION - Added: Mar-2022 #if AP_FENCE_ENABLED - AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, true); #endif static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_AIRSPEED_ENABLED // PARAMETER_CONVERSION - Added: JAN-2022 - { &airspeed, airspeed.var_info, 19, 4051 }, + { &airspeed, airspeed.var_info, 19 }, #endif #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 - { &stats, stats.var_info, 1, 4033 }, + { &stats, stats.var_info, 1 }, #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 - { &scripting, scripting.var_info, 18, 82 }, + { &scripting, scripting.var_info, 18 }, #endif #if AP_GRIPPER_ENABLED // PARAMETER_CONVERSION - Added: Feb-2024 - { &gripper, gripper.var_info, 3, 4035 }, + { &gripper, gripper.var_info, 3 }, #endif }; @@ -773,7 +773,7 @@ void Sub::load_parameters() // PARAMETER_CONVERSION - Added: Feb-2024 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif } From ee69b709d4b02454df85beb630ee7d8d355c224d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:32:00 +1100 Subject: [PATCH 33/52] Blimp: remove unused old_top_element param from convert_class --- Blimp/Parameters.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index b13b103af4f59..f9f11e44f0066 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -845,18 +845,18 @@ void Blimp::load_parameters(void) static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 - { &stats, stats.var_info, 12, 4044 }, + { &stats, stats.var_info, 12 }, #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 - { &scripting, scripting.var_info, 30, 94 }, + { &scripting, scripting.var_info, 30 }, #endif }; AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions)); // PARAMETER_CONVERSION - Added: Feb-2024 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif // setup AP_Param frame type flags From f673617b54227c07b391400d0a45cba02ab8907d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 08:32:00 +1100 Subject: [PATCH 34/52] Rover: remove unused old_top_element param from convert_class --- Rover/Parameters.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index a2a4b0f203bf7..5f24887181c8c 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -878,27 +878,27 @@ void Rover::load_parameters(void) static const AP_Param::G2ObjectConversion g2_conversions[] { #if AP_AIRSPEED_ENABLED // PARAMETER_CONVERSION - Added: JAN-2022 - { &airspeed, airspeed.var_info, 37, 4069 }, + { &airspeed, airspeed.var_info, 37 }, #endif #if AP_AIS_ENABLED // PARAMETER_CONVERSION - Added: MAR-2022 - { &ais, ais.var_info, 50, 114 }, + { &ais, ais.var_info, 50 }, #endif #if AP_FENCE_ENABLED // PARAMETER_CONVERSION - Added: Mar-2022 - { &fence, fence.var_info, 17, 4049 }, + { &fence, fence.var_info, 17 }, #endif #if AP_STATS_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 - { &stats, stats.var_info, 1, 4033 }, + { &stats, stats.var_info, 1 }, #endif #if AP_SCRIPTING_ENABLED // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 - { &scripting, scripting.var_info, 41, 105 }, + { &scripting, scripting.var_info, 41 }, #endif #if AP_GRIPPER_ENABLED // PARAMETER_CONVERSION - Added: Feb-2024 for Copter-4.6 - { &gripper, gripper.var_info, 39, 4071 }, + { &gripper, gripper.var_info, 39 }, #endif }; @@ -906,7 +906,7 @@ void Rover::load_parameters(void) // PARAMETER_CONVERSION - Added: Feb-2024 for Rover-4.6 #if HAL_LOGGING_ENABLED - AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, 0, true); + AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true); #endif } From cb2f764700cdb63c0eb95f302a3ad2e6e805895e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 14:41:27 +1100 Subject: [PATCH 35/52] AP_GPS: improve reporting of offset vs reported-distance inaccuracy --- libraries/AP_GPS/GPS_Backend.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 8147be24492cb..0af1b4ea479f9 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -369,8 +369,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) { // the magnitude of the vector is much further then we were expecting - Debug("Exceeded the permitted error margin %f > %f", - (double)(offset_dist - reported_distance), (double)(min_dist * permitted_error_length_pct)); + Debug("Offset=%.2f vs reported-distance=%.2f (max-delta=%.2f)", + offset_dist, reported_distance, (double)(min_dist * permitted_error_length_pct)); goto bad_yaw; } From e6a6db9ba0ab6106e72b89f873655d4d11747fba Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Feb 2024 15:54:49 +1100 Subject: [PATCH 36/52] AP_GPS: correct check of reported antenna distance vs stated offset --- libraries/AP_GPS/GPS_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 0af1b4ea479f9..95403f00905fa 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -367,7 +367,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, } - if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) { + if (fabsf(offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) { // the magnitude of the vector is much further then we were expecting Debug("Offset=%.2f vs reported-distance=%.2f (max-delta=%.2f)", offset_dist, reported_distance, (double)(min_dist * permitted_error_length_pct)); From a707628b163e51139a581c59c88ed04737b2c2c5 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Thu, 22 Feb 2024 20:59:50 -0500 Subject: [PATCH 37/52] AP_CANManager: Add multiCAN interface --- libraries/AP_CANManager/AP_CAN.h | 3 +- libraries/AP_CANManager/AP_CANDriver.cpp | 4 +- libraries/AP_CANManager/AP_CANSensor.cpp | 53 ++++++++++++++++++++++++ libraries/AP_CANManager/AP_CANSensor.h | 36 ++++++++++++++++ 4 files changed, 92 insertions(+), 4 deletions(-) diff --git a/libraries/AP_CANManager/AP_CAN.h b/libraries/AP_CANManager/AP_CAN.h index dff85215c15fa..c9befd5b0b219 100644 --- a/libraries/AP_CANManager/AP_CAN.h +++ b/libraries/AP_CANManager/AP_CAN.h @@ -28,7 +28,6 @@ class AP_CAN { Benewake = 11, Scripting2 = 12, TOFSenseP = 13, - NanoRadar_NRA24 = 14, - MR72 = 15, + NanoRadar = 14, }; }; diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 0ec7ebbceb2a0..782eea0e04568 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { // @Param: PROTOCOL // @DisplayName: Enable use of specific protocol over virtual driver // @Description: Enabling this option starts selected protocol that will use this virtual driver - // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24 + // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar // @User: Advanced // @RebootRequired: True AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, float(AP_CAN::Protocol::DroneCAN)), @@ -54,7 +54,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { // @Param: PROTOCOL2 // @DisplayName: Secondary protocol with 11 bit CAN addressing // @Description: Secondary protocol with 11 bit CAN addressing - // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24 + // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar // @User: Advanced // @RebootRequired: True AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)), diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 8e300eba0921b..17b46d1d30870 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -21,6 +21,7 @@ #include #include "AP_CANSensor.h" +#include extern const AP_HAL::HAL& hal; @@ -190,5 +191,57 @@ void CANSensor::loop() } } +MultiCAN::MultiCANLinkedList* MultiCAN::callbacks; + +MultiCAN::MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name) : + CANSensor(driver_name) +{ + if (callbacks == nullptr) { + callbacks = new MultiCANLinkedList(); + } + if (callbacks == nullptr) { + AP_BoardConfig::allocation_error("Failed to create multican callback"); + } + + // Register new driver + register_driver(can_type); + callbacks->register_callback(cf); +} + +// handle a received frame from the CAN bus +void MultiCAN::handle_frame(AP_HAL::CANFrame &frame) +{ + if (callbacks != nullptr) { + callbacks->handle_frame(frame); + } + +} + +// register a callback for a CAN frame by adding it to the linked list +void MultiCAN::MultiCANLinkedList::register_callback(ForwardCanFrame callback) +{ + CANSensor_Multi* newNode = new CANSensor_Multi(); + if (newNode == nullptr) { + AP_BoardConfig::allocation_error("Failed to create multican node"); + } + WITH_SEMAPHORE(sem); + { + newNode->_callback = callback; + newNode->next = head; + head = newNode; + } +} + +// distribute the CAN frame to the registered callbacks +void MultiCAN::MultiCANLinkedList::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(sem); + for (CANSensor_Multi* current = head; current != nullptr; current = current->next) { + if (current->_callback(frame)) { + return; + } + } +} + #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index 2cd775f4c39f7..7ba0021bb1346 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -90,5 +90,41 @@ class CANSensor : public AP_CANDriver { #endif }; +// a class to allow for multiple CAN backends with one +// CANSensor driver. This can be shared among different libraries like rangefinder and proximity +class MultiCAN : public CANSensor { +public: + // callback functor def for forwarding frames + FUNCTOR_TYPEDEF(ForwardCanFrame, bool, AP_HAL::CANFrame &); + + MultiCAN(ForwardCanFrame cf, AP_CAN::Protocol can_type, const char *driver_name); + + // handle a received frame from the CAN bus + void handle_frame(AP_HAL::CANFrame &frame) override; + +private: + // class to allow for multiple callbacks implemented as a linked list + class MultiCANLinkedList { + public: + struct CANSensor_Multi { + ForwardCanFrame _callback; + CANSensor_Multi* next = nullptr; + }; + + // register a callback for a CAN frame by adding it to the linked list + void register_callback(ForwardCanFrame callback); + + // distribute the CAN frame to the registered callbacks + void handle_frame(AP_HAL::CANFrame &frame); + HAL_Semaphore sem; + + private: + CANSensor_Multi* head = nullptr; + }; + + // Pointer to static instance of the linked list for persistence across instances + static MultiCANLinkedList* callbacks; +}; + #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS From 94fca60882b7691a16561b343742421f9ac62fca Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Thu, 22 Feb 2024 21:00:36 -0500 Subject: [PATCH 38/52] AP_Arming: Remove MR72 --- libraries/AP_Arming/AP_Arming.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 2a0a3e1fa0ac9..e6aaf5bb830bc 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1198,9 +1198,8 @@ bool AP_Arming::can_checks(bool report) } case AP_CAN::Protocol::USD1: case AP_CAN::Protocol::TOFSenseP: - case AP_CAN::Protocol::NanoRadar_NRA24: + case AP_CAN::Protocol::NanoRadar: case AP_CAN::Protocol::Benewake: - case AP_CAN::Protocol::MR72: { for (uint8_t j = i; j; j--) { if (AP::can().get_driver_type(i) == AP::can().get_driver_type(j-1)) { From 8a24699bfa6b48c0016de20eee43769fbf8ed92c Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Thu, 22 Feb 2024 21:01:03 -0500 Subject: [PATCH 39/52] AP_Proximity: move mutlican to AP_CANSensor --- .../AP_Proximity/AP_Proximity_MR72_CAN.cpp | 29 ++----------------- .../AP_Proximity/AP_Proximity_MR72_CAN.h | 19 +----------- 2 files changed, 4 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp index 4f0225c65bd6c..9d9618286c5b5 100644 --- a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp @@ -32,26 +32,14 @@ const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = { AP_GROUPEND }; -MR72_MultiCAN *AP_Proximity_MR72_CAN::multican; - AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params): AP_Proximity_Backend(_frontend, _state, _params) { - if (multican == nullptr) { - multican = new MR72_MultiCAN(); - if (multican == nullptr) { - AP_BoardConfig::allocation_error("MR72_CAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican->sem); - auto *prev = multican->drivers; - next = prev; - multican->drivers = this; + multican_MR72 = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_Proximity_MR72_CAN::handle_frame, bool, AP_HAL::CANFrame &), AP_CAN::Protocol::NanoRadar, "MR72 MultiCAN"}; + if (multican_MR72 == nullptr) { + AP_BoardConfig::allocation_error("Failed to create proximity multican"); } AP_Param::setup_object_defaults(this, var_info); @@ -132,15 +120,4 @@ bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame) return true; } -// handle frames from CANSensor, passing to the drivers -void MR72_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) -{ - WITH_SEMAPHORE(sem); - for (auto *d = drivers; d; d=d->next) { - if (d->handle_frame(frame)) { - break; - } - } -} - #endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h index e2a3dcbb9df36..210e8036b0864 100644 --- a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h @@ -45,24 +45,7 @@ class AP_Proximity_MR72_CAN : public AP_Proximity_Backend { AP_Int32 receive_id; // ID of the sensor - static MR72_MultiCAN *multican; // linked list - AP_Proximity_MR72_CAN *next; -}; - -// a class to allow for multiple MR_72 backends with one -// CANSensor driver -class MR72_MultiCAN : public CANSensor { -public: - MR72_MultiCAN() : CANSensor("MR72") { - register_driver(AP_CAN::Protocol::MR72); - } - - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; - - HAL_Semaphore sem; - AP_Proximity_MR72_CAN *drivers; - + MultiCAN* multican_MR72; // Allows for multiple CAN rangefinders on a single bus }; #endif // HAL_PROXIMITY_ENABLED From 89d8a1351c2b22cfdcc5391fb77423369494ed27 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Thu, 22 Feb 2024 21:01:25 -0500 Subject: [PATCH 40/52] AP_RangeFinder: move multican to AP_CANSensor --- .../AP_RangeFinder_Backend_CAN.cpp | 18 +++++-------- .../AP_RangeFinder_Backend_CAN.h | 20 +++------------ .../AP_RangeFinder_Benewake_CAN.cpp | 24 ------------------ .../AP_RangeFinder_Benewake_CAN.h | 8 +++--- .../AP_RangeFinder_NRA24_CAN.cpp | 22 ---------------- .../AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h | 8 +++--- .../AP_RangeFinder_TOFSenseP_CAN.cpp | 25 ------------------- .../AP_RangeFinder_TOFSenseP_CAN.h | 9 +++---- .../AP_RangeFinder_USD1_CAN.cpp | 24 ------------------ .../AP_RangeFinder/AP_RangeFinder_USD1_CAN.h | 11 +++----- 10 files changed, 25 insertions(+), 144 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp index 96c45d523870a..659a4af199106 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp @@ -39,11 +39,16 @@ const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = { // constructor AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN( - RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type, + const char *driver_name) : AP_RangeFinder_Backend(_state, _params) { AP_Param::setup_object_defaults(this, var_info); state.var_info = var_info; + multican_rangefinder = new MultiCAN{FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Backend_CAN::handle_frame, bool, AP_HAL::CANFrame &), can_type, driver_name}; + if (multican_rangefinder == nullptr) { + AP_BoardConfig::allocation_error("Failed to create rangefinder multican"); + } } // update the state of the sensor @@ -82,15 +87,4 @@ bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const return true; } -// handle frames from CANSensor, passing to the drivers -void RangeFinder_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) -{ - WITH_SEMAPHORE(sem); - for (auto *d = drivers; d != nullptr; d=d->next) { - if (d->handle_frame(frame)) { - break; - } - } -} - #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h index cc93613035291..96d62aa94dedf 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h @@ -14,7 +14,8 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend public: // constructor AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params); + AP_RangeFinder_Params &_params, AP_CAN::Protocol can_type, + const char *driver_name); friend class RangeFinder_MultiCAN; @@ -53,26 +54,11 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend AP_Int32 receive_id; // CAN ID to receive for this backend AP_Int32 snr_min; // minimum signal strength to accept packet + MultiCAN* multican_rangefinder; // Allows for multiple CAN rangefinders on a single bus private: float _distance_sum; // meters uint32_t _distance_count; }; -// a class to allow for multiple CAN backends with one -// CANSensor driver -class RangeFinder_MultiCAN : public CANSensor { -public: - RangeFinder_MultiCAN(AP_CAN::Protocol can_type, const char *driver_name) : CANSensor(driver_name) { - register_driver(can_type); - } - - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; - - // Semaphore for access to shared backend data - HAL_Semaphore sem; - AP_RangeFinder_Backend_CAN *drivers; -}; - #endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp index 180c114f2a14a..7f97dab592564 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -5,30 +5,6 @@ #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED -RangeFinder_MultiCAN *AP_RangeFinder_Benewake_CAN::multican; - -/* - constructor - */ -AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend_CAN(_state, _params) -{ - if (multican == nullptr) { - multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::Benewake, "Benewake MultiCAN"); - if (multican == nullptr) { - AP_BoardConfig::allocation_error("Benewake_CAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican->sem); - auto *prev = multican->drivers; - next = prev; - multican->drivers = this; - } -} - // handler for incoming frames for H30 radar bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index 6bc7057bedcd2..a5c6441258e58 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -7,14 +7,14 @@ class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN { public: - AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::Benewake, "benewake") + { + } // handler for incoming frames. Return true if consumed bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame_H30(AP_HAL::CANFrame &frame); - -private: - static RangeFinder_MultiCAN *multican; }; #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp index 1c922e121bbfc..30f170adddc9d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.cpp @@ -6,28 +6,6 @@ #include #include -RangeFinder_MultiCAN *AP_RangeFinder_NRA24_CAN::multican_NRA24; - -// constructor -AP_RangeFinder_NRA24_CAN::AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend_CAN(_state, _params) -{ - if (multican_NRA24 == nullptr) { - multican_NRA24 = new RangeFinder_MultiCAN(AP_CAN::Protocol::NanoRadar_NRA24, "NRA24 MultiCAN"); - if (multican_NRA24 == nullptr) { - AP_BoardConfig::allocation_error("Rangefinder_MultiCAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican_NRA24->sem); - auto *prev = multican_NRA24->drivers; - next = prev; - multican_NRA24->drivers = this; - } -} - // update the state of the sensor void AP_RangeFinder_NRA24_CAN::update(void) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h index 4a114be187d9a..eb0d472f3135e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h @@ -6,7 +6,10 @@ class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN { public: - AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + AP_RangeFinder_NRA24_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::NanoRadar, "nra24") + { + } void update(void) override; @@ -18,9 +21,6 @@ class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN { private: uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); } - - static RangeFinder_MultiCAN *multican_NRA24; - uint32_t last_heartbeat_ms; // last status message received from the sensor }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp index 5b9465a1dde7d..16497bb8d89c7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp @@ -7,31 +7,6 @@ #include #include -RangeFinder_MultiCAN *AP_RangeFinder_TOFSenseP_CAN::multican_TOFSenseP; - -/* - constructor - */ -AP_RangeFinder_TOFSenseP_CAN::AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend_CAN(_state, _params) -{ - if (multican_TOFSenseP == nullptr) { - multican_TOFSenseP = new RangeFinder_MultiCAN(AP_CAN::Protocol::TOFSenseP, "TOFSenseP MultiCAN"); - if (multican_TOFSenseP == nullptr) { - AP_BoardConfig::allocation_error("Rangefinder_MultiCAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican_TOFSenseP->sem); - auto *prev = multican_TOFSenseP->drivers; - next = prev; - multican_TOFSenseP->drivers = this; - } -} - - // handler for incoming frames. These come in at 10-30Hz bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h index a3100eef09dbe..ff88245ef54cd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h @@ -7,16 +7,15 @@ class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN { public: - AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::TOFSenseP, "tofsensep") + { + } // handler for incoming frames bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; - -private: - static RangeFinder_MultiCAN *multican_TOFSenseP; - }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp index 780308aabde6c..19964746c7527 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp @@ -4,30 +4,6 @@ #include -RangeFinder_MultiCAN *AP_RangeFinder_USD1_CAN::multican; - -/* - constructor - */ -AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend_CAN(_state, _params) -{ - if (multican == nullptr) { - multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::USD1, "USD1 MultiCAN"); - if (multican == nullptr) { - AP_BoardConfig::allocation_error("USD1_CAN"); - } - } - - { - // add to linked list of drivers - WITH_SEMAPHORE(multican->sem); - auto *prev = multican->drivers; - next = prev; - multican->drivers = this; - } -} - // handler for incoming frames. These come in at 100Hz bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h index 5a3827585ac30..445f7d3f50789 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h @@ -7,18 +7,15 @@ class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN { public: - - AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_CAN(_state, _params, AP_CAN::Protocol::USD1, "usd1") + { + } // handler for incoming frames bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; - -private: - - static RangeFinder_MultiCAN *multican; - }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED From c36b57acba227193fa470a1cf16174484c7d79b2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2024 19:29:09 +1100 Subject: [PATCH 41/52] AP_Compass: exclude cpp files if not AP_COMPASS_ENABLED --- libraries/AP_Compass/AP_Compass.cpp | 6 ++++++ libraries/AP_Compass/AP_Compass_Backend.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 45eccb97e7375..ad7397d0617eb 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1,3 +1,7 @@ +#include "AP_Compass_config.h" + +#if AP_COMPASS_ENABLED + #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include @@ -2236,3 +2240,5 @@ Compass &compass() } } + +#endif // AP_COMPASS_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index c0a7bf973eeaa..7d430c3193e19 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -1,3 +1,7 @@ +#include "AP_Compass_config.h" + +#if AP_COMPASS_ENABLED + #include #include "AP_Compass.h" @@ -294,3 +298,5 @@ enum Rotation AP_Compass_Backend::get_board_orientation(void) const { return _compass._board_orientation; } + +#endif // AP_COMPASS_ENABLED From 53fd9056d8e0324412b2b421b3764e5849fe3e22 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2024 19:29:10 +1100 Subject: [PATCH 42/52] AP_Scripting: use AP_ARMING_ENABLED and AP_COMPASS_ENABLED ... in place of periph-specific checks --- libraries/AP_Scripting/generator/description/bindings.desc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a5d1729c47378..c9e9867df5179 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -65,7 +65,7 @@ singleton AP_AHRS method get_quaternion boolean Quaternion'Null include AP_Arming/AP_Arming.h singleton AP_Arming rename arming -singleton AP_Arming depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_Arming depends AP_ARMING_ENABLED singleton AP_Arming method disarm boolean AP_Arming::Method::SCRIPTING'literal singleton AP_Arming method is_armed boolean singleton AP_Arming method pre_arm_checks boolean false'literal @@ -780,7 +780,7 @@ singleton AP_IOMCU method healthy boolean include AP_Compass/AP_Compass.h singleton Compass rename compass -singleton Compass depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_MAG)) +singleton Compass depends AP_COMPASS_ENABLED singleton Compass method healthy boolean uint8_t'skip_check -- ----EFI Library---- From df8801955db051cbf33f2b9b23cdc258cc819f78 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2024 19:35:39 +1100 Subject: [PATCH 43/52] AP_Vehicle: don't create compass singleton if compass not enabled --- libraries/AP_Vehicle/AP_Vehicle.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 24b542d891087..b2783f3cb50be 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -317,7 +317,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_GPS gps; #endif AP_Baro barometer; +#if AP_COMPASS_ENABLED Compass compass; +#endif #if AP_INERTIALSENSOR_ENABLED AP_InertialSensor ins; #endif From 5cc48a12e26bee6b7af5c82d0225fce9ec1aa5fb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2024 19:52:05 +1100 Subject: [PATCH 44/52] GCS_MAVLink: remove code based on ENABLE parameters --- libraries/GCS_MAVLink/GCS.cpp | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 65f799d118a40..a31c35f0115e5 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -331,7 +331,7 @@ void GCS::update_sensor_status_flags() // give GCS status of prearm checks. This is enabled if any arming checks are enabled. // it is healthy if armed or checks are passing -#if !defined(HAL_BUILD_AP_PERIPH) +#if AP_ARMING_ENABLED control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK; if (AP::arming().get_enabled_checks()) { control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ac7dd381335b4..4889e90b5d4d0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4530,14 +4530,16 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm AP::ins().force_save_calibration(); ret = MAV_RESULT_ACCEPTED; } -#endif +#endif // AP_INERTIALSENSOR_ENABLED +#if AP_COMPASS_ENABLED if (is_equal(packet.param2,76.0f)) { // force existing compass calibration to be accepted as valid AP::compass().force_save_calibration(); ret = MAV_RESULT_ACCEPTED; } - +#endif + return ret; } @@ -5747,6 +5749,7 @@ bool GCS_MAVLINK::send_relay_status() const void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const { +#if AP_AHRS_ENABLED // get attitude const AP_AHRS &ahrs = AP::ahrs(); Quaternion quat; @@ -5792,6 +5795,7 @@ void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const est_status_flags, // estimator status 0, // landed_state (see MAV_LANDED_STATE) AP::ahrs().get_yaw_rate_earth()); // [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown +#endif // AP_AHRS_ENABLED } void GCS_MAVLINK::send_received_message_deprecation_warning(const char * message) From 2e2b1c9fac502165d9a145197b14c2ad0cc454e8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Feb 2024 20:03:08 +1100 Subject: [PATCH 45/52] AP_HAL_ChibiOS: allow AP_TERRAIN_ENABLED to be turned off in hwdef adds the ifndef, and changes things to the path defines are undefined if it is false --- .../AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h index 718da256e0065..82d108d9024bd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_normal.h @@ -10,9 +10,18 @@ #define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" #endif -#ifndef HAL_BOARD_TERRAIN_DIRECTORY -#define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" +// a similar define is present in AP_HAL_Boards.h: +#ifndef HAL_OS_FATFS_IO +#define HAL_OS_FATFS_IO 0 #endif +#ifndef AP_TERRAIN_AVAILABLE // enable terrain only if there's an SD card available: #define AP_TERRAIN_AVAILABLE HAL_OS_FATFS_IO +#endif + +#if AP_TERRAIN_AVAILABLE +#ifndef HAL_BOARD_TERRAIN_DIRECTORY +#define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" +#endif +#endif // AP_TERRAIN_AVAILABLE From d4daa990f88300fc18662393038ced3d455431d3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 24 Feb 2024 12:34:42 +0000 Subject: [PATCH 46/52] AP_HAL_ChibiOS: hwdef.py: define `HAL_HAVE_AP_ROMFS_EMBEDDED_LUA` if lua files in ROMFS --- .../AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index a9d8521ef3076..bb6ca88d7861b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2625,6 +2625,11 @@ def write_hwdef_header(self, outfilename): self.embed_bootloader(f) if len(self.romfs) > 0: + # Allow lua to load from ROMFS if any lua files are added + for file in self.romfs.keys(): + if file.startswith("scripts") and file.endswith(".lua"): + f.write('#define HAL_HAVE_AP_ROMFS_EMBEDDED_LUA 1\n') + break f.write('#define HAL_HAVE_AP_ROMFS_EMBEDDED_H 1\n') if self.mcu_series.startswith('STM32F1'): @@ -3204,15 +3209,16 @@ def run(self): # write out hw.dat for ROMFS self.write_all_lines(os.path.join(self.outdir, "hw.dat")) + # Add ROMFS directories + self.romfs_add_dir(['scripts']) + self.romfs_add_dir(['param']) + # write out hwdef.h self.write_hwdef_header(os.path.join(self.outdir, "hwdef.h")) # write out ldscript.ld self.write_ldscript(os.path.join(self.outdir, "ldscript.ld")) - self.romfs_add_dir(['scripts']) - self.romfs_add_dir(['param']) - self.write_ROMFS(self.outdir) # copy the shared linker script into the build directory; it must From ec762d5609afee11c8585514323f8e5501868f83 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 24 Feb 2024 12:35:21 +0000 Subject: [PATCH 47/52] Tools: ardupilotwaf: boards: define `HAL_HAVE_AP_ROMFS_EMBEDDED_LUA` if lua files in ROMFS --- Tools/ardupilotwaf/boards.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index f699c5a75a573..1fd4bc7915ac8 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -770,6 +770,11 @@ def configure_env(self, cfg, env): env.ROMFS_FILES += [('scripts/'+f,'ROMFS/scripts/'+f)] if len(env.ROMFS_FILES) > 0: + # Allow lua to load from ROMFS if any lua files are added + for file in env.ROMFS_FILES: + if file[0].startswith("scripts") and file[0].endswith(".lua"): + env.CXXFLAGS += ['-DHAL_HAVE_AP_ROMFS_EMBEDDED_LUA'] + break env.CXXFLAGS += ['-DHAL_HAVE_AP_ROMFS_EMBEDDED_H'] if cfg.options.sitl_rgbled: @@ -1343,6 +1348,11 @@ def configure_env(self, cfg, env): HAL_PARAM_DEFAULTS_PATH='"@ROMFS/defaults.parm"', ) if len(env.ROMFS_FILES) > 0: + # Allow lua to load from ROMFS if any lua files are added + for file in env.ROMFS_FILES: + if file[0].startswith("scripts") and file[0].endswith(".lua"): + env.CXXFLAGS += ['-DHAL_HAVE_AP_ROMFS_EMBEDDED_LUA'] + break env.CXXFLAGS += ['-DHAL_HAVE_AP_ROMFS_EMBEDDED_H'] def build(self, bld): From c7d543f9af1dcb9bbcc66e1ecd6a8aa8ce8d8c42 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 24 Feb 2024 12:37:32 +0000 Subject: [PATCH 48/52] AP_Scripting: only try to load from ROMFS if `HAL_HAVE_AP_ROMFS_EMBEDDED_LUA` is defined --- libraries/AP_Scripting/lua_scripts.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index b1b60a29c8a71..98960a11ceba6 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -524,10 +524,12 @@ void lua_scripts::run(void) { load_all_scripts_in_dir(L, SCRIPTING_DIRECTORY); loaded = true; } +#ifdef HAL_HAVE_AP_ROMFS_EMBEDDED_LUA if ((dir_disable & uint16_t(AP_Scripting::SCR_DIR::ROMFS)) == 0) { load_all_scripts_in_dir(L, "@ROMFS/scripts"); loaded = true; } +#endif if (!loaded) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Lua: All directory's disabled see SCR_DIR_DISABLE"); } From fee5374cb66cfb80dcfe5ba02ac51ab3c3a01e0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 25 Feb 2024 08:06:43 +1100 Subject: [PATCH 49/52] Tools: only produce _with_bl.hex not *.hex for vehicle firmware if we are producing _with_bl.hex then don't also build *.hex as it can confuse users who think they should flash *.hex with a DFU flashing tool --- Tools/ardupilotwaf/chibios.py | 5 ++++- Tools/scripts/make_intel_hex.py | 17 +++++++++-------- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 0248c1c25b5d1..532e75a4b3a33 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -435,7 +435,10 @@ def chibios_firmware(self): bootloader_bin = self.bld.srcnode.make_node("Tools/bootloaders/%s_bl.bin" % self.env.BOARD) if self.bld.env.HAVE_INTEL_HEX: if os.path.exists(bootloader_bin.abspath()): - hex_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.hex').name) + if int(self.bld.env.FLASH_RESERVE_START_KB) > 0: + hex_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('_with_bl.hex').name) + else: + hex_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.hex').name) hex_task = self.create_task('build_intel_hex', src=[bin_target[0], bootloader_bin], tgt=hex_target) hex_task.set_run_after(cleanup_task) else: diff --git a/Tools/scripts/make_intel_hex.py b/Tools/scripts/make_intel_hex.py index 629ebf34b5e22..6d9802b712d7b 100755 --- a/Tools/scripts/make_intel_hex.py +++ b/Tools/scripts/make_intel_hex.py @@ -39,13 +39,14 @@ tmpfile = hexfile + ".tmp" -open(tmpfile, "wb").write(appimage) - -intelhex.bin2hex(tmpfile, hexfile, offset=(0x08000000 + reserve_kb*1024)) - +# we either write a _with_bl.hex or a .hex. We don't write both as users get +# confused if offered a arducopter.hex and try to flash it with INAV DFU +# flashing tool, not realising it has to be flashed at the correct flash offset if reserve_kb > 0: - open(tmpfile, "wb").write(with_bl) - intelhex.bin2hex(tmpfile, hex_with_bl, offset=0x08000000) - -os.unlink(tmpfile) + open(tmpfile, "wb").write(with_bl) + intelhex.bin2hex(tmpfile, hex_with_bl, offset=0x08000000) +else: + open(tmpfile, "wb").write(appimage) + intelhex.bin2hex(tmpfile, hexfile, offset=(0x08000000 + reserve_kb*1024)) +os.unlink(tmpfile) From 744df1ceba94cd31160e56b1fa43acb9cafccc67 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 24 Feb 2024 14:28:00 +0000 Subject: [PATCH 50/52] AP_Motors: Tricopter: fix actuator indexing --- libraries/AP_Motors/AP_MotorsTri.cpp | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 7b868d3aaf4bf..43730335fd584 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -90,34 +90,32 @@ void AP_MotorsTri::output_to_motors() switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors - rc_write(AP_MOTORS_MOT_1, output_to_pwm(0)); - rc_write(AP_MOTORS_MOT_2, output_to_pwm(0)); - rc_write(AP_MOTORS_MOT_4, output_to_pwm(0)); + _actuator[AP_MOTORS_MOT_1] = 0.0; + _actuator[AP_MOTORS_MOT_2] = 0.0; + _actuator[AP_MOTORS_MOT_4] = 0.0; rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); break; case SpoolState::GROUND_IDLE: // sends output to motors when armed but not flying - set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle()); - set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle()); - set_actuator_with_slew(_actuator[4], actuator_spin_up_to_ground_idle()); - rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1])); - rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2])); - rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4])); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], actuator_spin_up_to_ground_idle()); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], actuator_spin_up_to_ground_idle()); rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); break; case SpoolState::SPOOLING_UP: case SpoolState::THROTTLE_UNLIMITED: case SpoolState::SPOOLING_DOWN: // set motor output based on thrust requests - set_actuator_with_slew(_actuator[1], thr_lin.thrust_to_actuator(_thrust_right)); - set_actuator_with_slew(_actuator[2], thr_lin.thrust_to_actuator(_thrust_left)); - set_actuator_with_slew(_actuator[4], thr_lin.thrust_to_actuator(_thrust_rear)); - rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1])); - rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2])); - rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4])); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_1], thr_lin.thrust_to_actuator(_thrust_right)); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_2], thr_lin.thrust_to_actuator(_thrust_left)); + set_actuator_with_slew(_actuator[AP_MOTORS_MOT_4], thr_lin.thrust_to_actuator(_thrust_rear)); rc_write_angle(AP_MOTORS_CH_TRI_YAW, degrees(_pivot_angle)*100); break; } + + rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[AP_MOTORS_MOT_1])); + rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[AP_MOTORS_MOT_2])); + rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[AP_MOTORS_MOT_4])); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) From 2df3cb98c68e65183c433917dc2e4198ffe7daba Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 26 Feb 2024 14:10:35 +1100 Subject: [PATCH 51/52] AP_NavEKF3: move initialisation of rngBcn into BeaconFusion method --- .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 43 +++++++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 39 +---------------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 2 + 3 files changed, 46 insertions(+), 38 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 9708b60f164ef..714fc41711e91 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -3,6 +3,49 @@ #if EK3_FEATURE_BEACON_FUSION +// initialise state: +void NavEKF3_core::BeaconFusion::InitialiseVariables() +{ + memset((void *)&dataDelayed, 0, sizeof(dataDelayed)); + lastPassTime_ms = 0; + testRatio = 0.0f; + health = false; + varInnov = 0.0f; + innov = 0.0f; + memset(&lastTime_ms, 0, sizeof(lastTime_ms)); + dataToFuse = false; + vehiclePosNED.zero(); + vehiclePosErr = 1.0f; + last3DmeasTime_ms = 0; + goodToAlign = false; + lastChecked = 0; + receiverPos.zero(); + memset(&receiverPosCov, 0, sizeof(receiverPosCov)); + alignmentStarted = false; + alignmentCompleted = false; + lastIndex = 0; + posSum.zero(); + numMeas = 0; + sum = 0.0f; + N = 0; + maxPosD = 0.0f; + minPosD = 0.0f; + posDownOffsetMax = 0.0f; + posOffsetMaxVar = 0.0f; + maxOffsetStateChangeFilt = 0.0f; + posDownOffsetMin = 0.0f; + posOffsetMinVar = 0.0f; + minOffsetStateChangeFilt = 0.0f; + fuseDataReportIndex = 0; + if (AP::dal().beacon()) { + if (fusionReport == nullptr) { + fusionReport = new BeaconFusion::FusionReport[AP::dal().beacon()->count()]; + } + } + posOffsetNED.zero(); + originEstInit = false; +} + /******************************************************** * FUSE MEASURED_DATA * ********************************************************/ diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 1d2316f966219..ae1f4d09a14e7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -345,44 +345,7 @@ void NavEKF3_core::InitialiseVariables() // range beacon fusion variables #if EK3_FEATURE_BEACON_FUSION - memset((void *)&rngBcn.dataDelayed, 0, sizeof(rngBcn.dataDelayed)); - rngBcn.lastPassTime_ms = 0; - rngBcn.testRatio = 0.0f; - rngBcn.health = false; - rngBcn.varInnov = 0.0f; - rngBcn.innov = 0.0f; - memset(&rngBcn.lastTime_ms, 0, sizeof(rngBcn.lastTime_ms)); - rngBcn.dataToFuse = false; - rngBcn.vehiclePosNED.zero(); - rngBcn.vehiclePosErr = 1.0f; - rngBcn.last3DmeasTime_ms = 0; - rngBcn.goodToAlign = false; - rngBcn.lastChecked = 0; - rngBcn.receiverPos.zero(); - memset(&rngBcn.receiverPosCov, 0, sizeof(rngBcn.receiverPosCov)); - rngBcn.alignmentStarted = false; - rngBcn.alignmentCompleted = false; - rngBcn.lastIndex = 0; - rngBcn.posSum.zero(); - rngBcn.numMeas = 0; - rngBcn.sum = 0.0f; - rngBcn.N = 0; - rngBcn.maxPosD = 0.0f; - rngBcn.minPosD = 0.0f; - rngBcn.posDownOffsetMax = 0.0f; - rngBcn.posOffsetMaxVar = 0.0f; - rngBcn.maxOffsetStateChangeFilt = 0.0f; - rngBcn.posDownOffsetMin = 0.0f; - rngBcn.posOffsetMinVar = 0.0f; - rngBcn.minOffsetStateChangeFilt = 0.0f; - rngBcn.fuseDataReportIndex = 0; - if (dal.beacon()) { - if (rngBcn.fusionReport == nullptr) { - rngBcn.fusionReport = new BeaconFusion::FusionReport[dal.beacon()->count()]; - } - } - rngBcn.posOffsetNED.zero(); - rngBcn.originEstInit = false; + rngBcn.InitialiseVariables(); #endif // EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BODY_ODOM diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8ba964b7e88fd..d9304f12383cf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1323,6 +1323,8 @@ class NavEKF3_core : public NavEKF_core_common #if EK3_FEATURE_BEACON_FUSION class BeaconFusion { public: + void InitialiseVariables(); + EKF_obs_buffer_t storedRange; // Beacon range buffer rng_bcn_elements dataDelayed; // Range beacon data at the fusion time horizon uint32_t lastPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec) From 242f679206b9025225794962cf3c75385ad35498 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 26 Feb 2024 11:26:20 +0000 Subject: [PATCH 52/52] AP_HAL_ChibiOS: remove iomcu dshot from Pixhawk1-1M and add to Pixhawk1-bdshot add support for removing ROMFS from hwdef --- libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat | 5 +++++ libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-bdshot/hwdef.dat | 4 ++++ libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 2 ++ 3 files changed, 11 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat index 096e1fdfd5ec1..b0c52d2c35225 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat @@ -13,3 +13,8 @@ undef STORAGE_FLASH_PAGE # produce this error if we are on a 2M board and using 1M firmware define BOARD_CHECK_F427_USE_2M "2M flash - use Pixhawk1 firmware" + +# enable support for dshot on iomcu +undef ROMFS +ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_lowpolh.bin +undef HAL_WITH_IO_MCU_DSHOT \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-bdshot/hwdef.dat index 67ea0aae84d5d..3aca8a528df99 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-bdshot/hwdef.dat @@ -36,3 +36,7 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES # produce this error if we are on a 1M board undef BOARD_CHECK_F427_USE_1M define BOARD_CHECK_F427_USE_1M "ERROR: 1M flash use Pixhawk1-1M" + +# enable support for dshot on iomcu +ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin +define HAL_WITH_IO_MCU_DSHOT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index bb6ca88d7861b..1a3e5adc224c5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -3054,6 +3054,8 @@ def process_line(self, line): self.baro_list = [] if u == 'AIRSPEED': self.airspeed_list = [] + if u == 'ROMFS': + self.romfs = {} elif a[0] == 'env': self.progress("Adding environment %s" % ' '.join(a[1:])) if len(a[1:]) < 2: