From be49a06f0412e4cd25b5d9077f12aa669b70d129 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Sep 2024 09:56:16 +0100 Subject: [PATCH 01/26] AP_Vehicle: remove runcam singleton --- libraries/AP_Vehicle/AP_Vehicle.cpp | 13 ++----------- libraries/AP_Vehicle/AP_Vehicle.h | 4 ---- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 9f7d70bb24..4b6a95807f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -35,11 +35,8 @@ extern AP_IOMCU iomcu; 2nd group of parameters */ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { -#if HAL_RUNCAM_ENABLED - // @Group: CAM_RC_ - // @Path: ../AP_Camera/AP_RunCam.cpp - AP_SUBGROUPINFO(runcam, "CAM_RC_", 1, AP_Vehicle, AP_RunCam), -#endif + + // 1: RunCam #if HAL_GYROFFT_ENABLED // @Group: FFT_ @@ -450,9 +447,6 @@ void AP_Vehicle::setup() gyro_fft.init(1000); #endif #endif -#if HAL_RUNCAM_ENABLED - runcam.init(); -#endif #if HAL_HOTT_TELEM_ENABLED hott_telem.init(); #endif @@ -615,9 +609,6 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if HAL_NMEA_OUTPUT_ENABLED SCHED_TASK_CLASS(AP_NMEA_Output, &vehicle.nmea, update, 50, 50, 180), #endif -#if HAL_RUNCAM_ENABLED - SCHED_TASK_CLASS(AP_RunCam, &vehicle.runcam, update, 50, 50, 200), -#endif #if HAL_GYROFFT_ENABLED SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210), diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index f34be5accb..f68350c8ca 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -47,7 +47,6 @@ #include #include // Serial manager library #include -#include #include #include #include @@ -373,9 +372,6 @@ protected: AP_RSSI rssi; #endif -#if HAL_RUNCAM_ENABLED - AP_RunCam runcam; -#endif #if HAL_GYROFFT_ENABLED AP_GyroFFT gyro_fft; #endif From 9308a6ef6938a616514418389822934c2b254d23 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Sep 2024 16:48:10 +0100 Subject: [PATCH 02/26] RC_Channel: HAL_RUNCAM_ENABLED -> AP_CAMERA_RUNCAM_ENABLED --- libraries/RC_Channel/RC_Channel.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index d6f0d90b4a..a30c16f4c9 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -742,7 +742,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos #endif case AUX_FUNC::MOTOR_ESTOP: case AUX_FUNC::RC_OVERRIDE_ENABLE: -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED case AUX_FUNC::RUNCAM_CONTROL: case AUX_FUNC::RUNCAM_OSD_CONTROL: #endif @@ -849,7 +849,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { #endif { AUX_FUNC::SAILBOAT_MOTOR_3POS,"SailboatMotor"}, { AUX_FUNC::SURFACE_TRACKING,"SurfaceTracking"}, -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED { AUX_FUNC::RUNCAM_CONTROL,"RunCamControl"}, { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"}, #endif @@ -1169,7 +1169,7 @@ bool RC_Channel::do_aux_function_camera_lens(const AuxSwitchPos ch_flag) } #endif // AP_CAMERA_ENABLED -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag) { AP_RunCam *runcam = AP::runcam(); @@ -1474,7 +1474,7 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch break; #endif // AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED case AUX_FUNC::RUNCAM_CONTROL: do_aux_function_runcam_control(ch_flag); break; From 00c9d013ddd1c1fe5f61dfd86afad8be652dad1a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Sep 2024 16:48:42 +0100 Subject: [PATCH 03/26] scripts: HAL_RUNCAM_ENABLED -> AP_CAMERA_RUNCAM_ENABLED --- Tools/scripts/build_options.py | 2 +- Tools/scripts/extract_features.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index b8ee3e03cb..659d8c5cd4 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -146,7 +146,7 @@ BUILD_OPTIONS = [ Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable GCS camera thermal range', 0, 'Camera,MOUNT'), # noqa: E501 Feature('Camera', 'Camera_Info_From_Script', 'AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'Enable Camera information messages via Lua script', 0, 'Camera,SCRIPTING'), # noqa - Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam control', 0, None), + Feature('Camera', 'RUNCAM', 'AP_CAMERA_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None), Feature('Copter', 'MODE_ZIGZAG', 'MODE_ZIGZAG_ENABLED', 'Enable Mode ZigZag', 0, None), Feature('Copter', 'MODE_SYSTEMID', 'MODE_SYSTEMID_ENABLED', 'Enable Mode SystemID', 0, 'Logging'), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 48e57767a1..4224f2de4b 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -138,7 +138,7 @@ class ExtractFeatures(object): ('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'), ('AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'AP_Camera::send_camera_thermal_range'), ('AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'AP_Camera_Backend::set_camera_information'), - ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',), + ('AP_CAMERA_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',), ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',), ('AP_PROXIMITY_{type}_ENABLED', 'AP_Proximity_(?P.*)::update',), From ee56d90f959381578766319d60527d1e8d43e26c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Sep 2024 16:47:31 +0100 Subject: [PATCH 04/26] AP_Arming: HAL_RUNCAM_ENABLED -> AP_CAMERA_RUNCAM_ENABLED --- libraries/AP_Arming/AP_Arming.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 4b058abe53..cc6e93dab3 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1291,7 +1291,7 @@ bool AP_Arming::fence_checks(bool display_failure) } #endif // AP_FENCE_ENABLED -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED bool AP_Arming::camera_checks(bool display_failure) { if (check_enabled(ARMING_CHECK_CAMERA)) { @@ -1309,7 +1309,7 @@ bool AP_Arming::camera_checks(bool display_failure) } return true; } -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED #if OSD_ENABLED bool AP_Arming::osd_checks(bool display_failure) const @@ -1603,7 +1603,7 @@ bool AP_Arming::pre_arm_checks(bool report) #if HAL_PROXIMITY_ENABLED & proximity_checks(report) #endif -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED & camera_checks(report) #endif #if OSD_ENABLED From 7a08f2bf3dcb97d69c7cd3692a39b1fb92651a7a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 10 Nov 2024 12:13:48 +0000 Subject: [PATCH 05/26] AP_Param: add group idx calculator --- libraries/AP_Param/AP_Param.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 2b2a4094ff..79b0aca7f2 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -173,7 +173,7 @@ #define GOBJECTN(v, pname, name, class) { name, (const void *)&AP_PARAM_VEHICLE_NAME.v, {group_info : class::var_info}, 0, Parameters::k_param_ ## pname, AP_PARAM_GROUP } #define PARAM_VEHICLE_INFO { "", (const void *)&AP_PARAM_VEHICLE_NAME, {group_info : AP_Vehicle::var_info}, 0, Parameters::k_param_vehicle, AP_PARAM_GROUP } #define AP_VAREND { "", nullptr, {group_info : nullptr }, 0, 0, AP_PARAM_NONE } - +#define AP_GROUP_ELEM_IDX(subgrp_idx, grp_idx) (grp_idx << 6 | subgrp_idx) enum ap_var_type { AP_PARAM_NONE = 0, From 90fc426fd5e6de0674ec8011c08be8ba02980b52 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 25 Nov 2024 18:30:57 +0000 Subject: [PATCH 06/26] AP_HAL_ChibiOS: ensure RunCam backend gets included in minimal --- libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index 3c6e149ee0..bd666a4957 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -48,6 +48,7 @@ define AP_WINCH_ENABLED 0 define AP_CAMERA_BACKEND_DEFAULT_ENABLED 0 define AP_CAMERA_RELAY_ENABLED AP_CAMERA_ENABLED define AP_CAMERA_SERVO_ENABLED AP_CAMERA_ENABLED +define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_ENABLED # no SLCAN on these boards (use can-over-mavlink if required) define AP_CAN_SLCAN_ENABLED 0 From 399f9f6f9805582dfb7208d8f9317b7b30c0ab5c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Apr 2024 17:58:56 +0100 Subject: [PATCH 07/26] AP_Camera: RunCam camera backend RunCam parameter conversion add RunCam camera settings and control --- libraries/AP_Camera/AP_Camera.cpp | 69 +++++++++++- libraries/AP_Camera/AP_Camera.h | 10 ++ libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- libraries/AP_Camera/AP_Camera_config.h | 4 + libraries/AP_Camera/AP_RunCam.cpp | 127 +++++++++++++++++++---- libraries/AP_Camera/AP_RunCam.h | 49 +++++++-- 6 files changed, 232 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 288b0506bb..aee97439dc 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "AP_Camera_Backend.h" #include "AP_Camera_Servo.h" #include "AP_Camera_Relay.h" @@ -14,6 +15,7 @@ #include "AP_Camera_MAVLink.h" #include "AP_Camera_MAVLinkCamV2.h" #include "AP_Camera_Scripting.h" +#include "AP_RunCam.h" const AP_Param::GroupInfo AP_Camera::var_info[] = { @@ -41,10 +43,24 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Path: AP_Camera_Params.cpp AP_SUBGROUPINFO(_params[1], "2", 13, AP_Camera, AP_Camera_Params), #endif +#if AP_CAMERA_RUNCAM_ENABLED + // @Group: 1_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[0], "1_RC_", 14, AP_Camera, _backend_var_info[0]), +#if AP_CAMERA_MAX_INSTANCES > 1 + // @Group: 2_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[1], "2_RC_", 15, AP_Camera, _backend_var_info[1]), +#endif +#endif AP_GROUPEND }; +#if AP_CAMERA_RUNCAM_ENABLED +const AP_Param::GroupInfo *AP_Camera::_backend_var_info[AP_CAMERA_MAX_INSTANCES]; +#endif + extern const AP_HAL::HAL& hal; AP_Camera::AP_Camera(uint32_t _log_camera_bit) : @@ -238,6 +254,17 @@ void AP_Camera::init() case CameraType::SCRIPTING: _backends[instance] = NEW_NOTHROW AP_Camera_Scripting(*this, _params[instance], instance); break; +#endif +#if AP_CAMERA_RUNCAM_ENABLED + // check for RunCam driver + case CameraType::RUNCAM: + if (_backends[instance] == nullptr) { // may have already been created by the conversion code + _backends[instance] = NEW_NOTHROW AP_RunCam(*this, _params[instance], instance, _runcam_instances); + _backend_var_info[instance] = AP_RunCam::var_info; + AP_Param::load_object_from_eeprom(_backends[instance], _backend_var_info[instance]); + _runcam_instances++; + } + break; #endif case CameraType::NONE: break; @@ -899,7 +926,11 @@ AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const void AP_Camera::convert_params() { // exit immediately if CAM1_TYPE has already been configured - if (_params[0].type.configured()) { + if (_params[0].type.configured() +#if AP_CAMERA_RUNCAM_ENABLED + && _params[1].type.configured() +#endif + ) { return; } @@ -919,6 +950,42 @@ void AP_Camera::convert_params() } _params[0].type.set_and_save(cam1_type); +#if AP_CAMERA_RUNCAM_ENABLED + // RunCam PARAMETER_CONVERSION - Added: Nov-2024 ahead of 4.7 release + + // Since slot 1 is essentially used by the trigger type, we will use slot 2 for runcam + int8_t rc_type = 0; + // find vehicle's top level key + uint16_t k_param_vehicle_key; + if (!AP_Param::find_top_level_key_by_pointer(AP::vehicle(), k_param_vehicle_key)) { + return; + } + + // RunCam protocol configured so set cam type to RunCam + bool rc_protocol_configured = false; + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + if (serial_manager && serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0)) { + rc_protocol_configured = true; + } + + const AP_Param::ConversionInfo rc_type_info = { + k_param_vehicle_key, AP_GROUP_ELEM_IDX(1, 1), AP_PARAM_INT8, "CAM_RC_TYPE" + }; + AP_Int8 rc_type_old; + const bool found_rc_type = AP_Param::find_old_parameter(&rc_type_info, &rc_type_old); + + if (rc_protocol_configured || (found_rc_type && rc_type_old.get() > 0)) { + rc_type = int8_t(CameraType::RUNCAM); + _backends[1] = NEW_NOTHROW AP_RunCam(*this, _params[1], 1, _runcam_instances); + _backend_var_info[1] = AP_RunCam::var_info; + AP_Param::convert_class(k_param_vehicle_key, &_backends[1], _backend_var_info[1], 1, false); + AP_Param::invalidate_count(); + _runcam_instances++; + } + + _params[1].type.set_and_save(rc_type); +#endif // AP_CAMERA_RUNCAM_ENABLED + // convert CAM_DURATION (in deci-seconds) to CAM1_DURATION (in seconds) int8_t cam_duration = 0; if (AP_Param::get_param_by_index(this, 1, AP_PARAM_INT8, &cam_duration) && (cam_duration > 0)) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a7b7f602ac..e1f0a07433 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -23,6 +23,7 @@ class AP_Camera_Mount; class AP_Camera_MAVLink; class AP_Camera_MAVLinkCamV2; class AP_Camera_Scripting; +class AP_RunCam; /// @class Camera /// @brief Object managing a Photo or video camera @@ -37,6 +38,7 @@ class AP_Camera { friend class AP_Camera_MAVLink; friend class AP_Camera_MAVLinkCamV2; friend class AP_Camera_Scripting; + friend class AP_RunCam; public: @@ -72,6 +74,9 @@ public: #endif #if AP_CAMERA_SCRIPTING_ENABLED SCRIPTING = 7, // Scripting backend +#endif +#if AP_CAMERA_RUNCAM_ENABLED + RUNCAM = 8, // RunCam backend #endif }; @@ -216,6 +221,11 @@ protected: // parameters for backends AP_Camera_Params _params[AP_CAMERA_MAX_INSTANCES]; +#if AP_CAMERA_RUNCAM_ENABLED + // var info pointer for RunCam + static const struct AP_Param::GroupInfo *_backend_var_info[AP_CAMERA_MAX_INSTANCES]; + uint8_t _runcam_instances; +#endif private: diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 749d2985f0..7c4b092996 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting, 8:RunCam // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 90f69ea7da..fa1ac3c487 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -62,3 +62,7 @@ #ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED #define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED #endif + +#ifndef AP_CAMERA_RUNCAM_ENABLED +#define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_RUNCAM_ENABLED +#endif diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp index fadecee6d0..c6d461667b 100644 --- a/libraries/AP_Camera/AP_RunCam.cpp +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -23,7 +23,7 @@ */ #include "AP_RunCam.h" -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -36,7 +36,7 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam device type // @Description: RunCam device type used to determine OSD menu structure and shutter options. // @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split, 3:RunCam Split4 4k, 4:RunCam Hybrid/RunCam Thumb Pro, 5:Runcam 2 4k - AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceType::Disabled), AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceModel::SplitMicro), AP_PARAM_FLAG_ENABLE), // @Param: FEATURES // @DisplayName: RunCam features available @@ -55,13 +55,13 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam button delay before allowing further button presses // @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("BTN_DELAY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), + AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), // @Param: MDE_DELAY // @DisplayName: RunCam mode delay before allowing further button presses // @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("MDE_DELAY", 5, AP_RunCam, _mode_delay_ms, 800), + AP_GROUPINFO("MDE_DELY", 5, AP_RunCam, _mode_delay_ms, 800), // @Param: CONTROL // @DisplayName: RunCam control option @@ -118,13 +118,24 @@ AP_RunCam::Menu AP_RunCam::_menus[RUNCAM_MAX_DEVICE_TYPES] = { { 6, { 3, 10, 2, 2, 8 }}, // Runcam 2 4K }; -AP_RunCam::AP_RunCam() +const char* AP_RunCam::_models[RUNCAM_MAX_DEVICE_TYPES] = { + "SplitMicro", + "Split", + "Split4k", + "Hybrid", + "Run24k" +}; + +AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance) + : AP_Camera_Backend(frontend, params, instance), _runcam_instance(runcam_instance) { AP_Param::setup_object_defaults(this, var_info); - if (_singleton != nullptr) { - AP_HAL::panic("AP_RunCam must be singleton"); + if (_singleton != nullptr && _singleton->_instance == instance) { + AP_HAL::panic("AP_RunCam instance must be a singleton %u\n", instance); + } + if (_singleton == nullptr) { + _singleton = this; } - _singleton = this; _cam_type.set(constrain_int16(_cam_type, 0, RUNCAM_MAX_DEVICE_TYPES)); _video_recording = VideoOption(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT)); } @@ -134,19 +145,19 @@ void AP_RunCam::init() { AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); if (serial_manager) { - uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0); + uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, _runcam_instance); } if (uart != nullptr) { /* if the user has setup a serial port as a runcam then default type to the split micro (Andy's development platform!). This makes setup a bit easier for most users while still enabling parameters to be hidden for users - without a runcam + without a RunCam */ - _cam_type.set_default(int8_t(DeviceType::SplitMicro)); + _cam_type.set_default(int8_t(DeviceModel::SplitMicro)); AP_Param::invalidate_count(); } - if (_cam_type.get() == int8_t(DeviceType::Disabled)) { + if (_cam_type.get() == int8_t(DeviceModel::Disabled)) { uart = nullptr; return; } @@ -156,7 +167,7 @@ void AP_RunCam::init() } // Split and Runcam 2 4k requires two mode presses to get into the menu - if (_cam_type.get() == int8_t(DeviceType::Split) || _cam_type.get() == int8_t(DeviceType::Run24k)) { + if (_cam_type.get() == int8_t(DeviceModel::Split) || _cam_type.get() == int8_t(DeviceModel::Run24k)) { _menu_enter_level = -1; _in_menu = -1; } @@ -221,7 +232,7 @@ void AP_RunCam::osd_option() { // input update loop void AP_RunCam::update() { - if (uart == nullptr || _cam_type.get() == int8_t(DeviceType::Disabled)) { + if (uart == nullptr || _cam_type.get() == int8_t(DeviceModel::Disabled)) { return; } @@ -551,12 +562,12 @@ void AP_RunCam::handle_2_key_simulation_process(Event ev) case Event::IN_MENU_ENTER: // in a sub-menu and save-and-exit was selected - if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms); _sub_menu_pos = 0; _in_menu--; // in the top-menu and save-and-exit was selected - } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _mode_delay_ms); _in_menu--; _state = State::EXITING_MENU; @@ -712,7 +723,7 @@ void AP_RunCam::handle_5_key_simulation_response(const Request& request) // command to start recording AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING; @@ -721,7 +732,7 @@ AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { // command to stop recording AP_RunCam::ControlOperation AP_RunCam::stop_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING; @@ -1058,8 +1069,86 @@ uint8_t AP_RunCam::Request::get_expected_response_length(const Command command) return 0; } +// AP_Camera API + +// return true if healthy +bool AP_RunCam::healthy() const +{ + return camera_ready(); +} + +// momentary switch to change camera between picture and video modes +void AP_RunCam::cam_mode_toggle() { + +} + +// entry point to actually take a picture. returns true on success +bool AP_RunCam::trigger_pic() { + return false; +} + +// send camera information message to GCS +void AP_RunCam::send_camera_information(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready() || _cam_type.get() == 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) { + return; + } + + static const uint8_t vendor_name[32] = "RunCam"; + uint8_t model_name[32] {}; + strncpy((char *)model_name, _models[_cam_type.get()-1], MIN(sizeof(model_name), sizeof(_models[_cam_type.get()-1]))); + const char cam_definition_uri[140] {}; + + // capability flags + uint32_t flags = 0; + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) { + flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO; + } + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) { + flags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE; + } + + // send CAMERA_INFORMATION message + mavlink_msg_camera_information_send( + chan, + AP_HAL::millis(), // time_boot_ms + vendor_name, // vendor_name uint8_t[32] + model_name, // model_name uint8_t[32] + 0, // firmware version uint32_t + NaNf, // sensor_size_h float (mm) + NaNf, // sensor_size_v float (mm) + 0, // sensor_size_v float (mm) + 0, // resolution_h uint16_t (pix) + 0, // resolution_v uint16_t (pix) + 0, // lens_id uint8_t + flags, // flags uint32_t (CAMERA_CAP_FLAGS) + 0, // cam_definition_version uint16_t + cam_definition_uri, // cam_definition_uri char[140] + _instance + 1); // gimbal_device_id uint8_t +} + +// send camera settings message to GCS +void AP_RunCam::send_camera_settings(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready()) { + return; + } + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _video_recording == VideoOption::RECORDING ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + AP_RunCam *AP::runcam() { return AP_RunCam::get_singleton(); } -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED diff --git a/libraries/AP_Camera/AP_RunCam.h b/libraries/AP_Camera/AP_RunCam.h index 9eb6299923..27b329fb68 100644 --- a/libraries/AP_Camera/AP_RunCam.h +++ b/libraries/AP_Camera/AP_RunCam.h @@ -23,7 +23,9 @@ #include "AP_Camera_config.h" -#if HAL_RUNCAM_ENABLED +#include "AP_Camera_Backend.h" + +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -38,10 +40,10 @@ /// @class AP_RunCam /// @brief Object managing a RunCam device -class AP_RunCam +class AP_RunCam : public AP_Camera_Backend { public: - AP_RunCam(); + AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance); // do not allow copies CLASS_NO_COPY(AP_RunCam); @@ -51,7 +53,7 @@ public: return _singleton; } - enum class DeviceType { + enum class DeviceModel { Disabled = 0, SplitMicro = 1, // video support only Split = 2, // camera and video support @@ -79,22 +81,49 @@ public: VIDEO_RECORDING_AT_BOOT = (1 << 4) }; + + // return true if healthy + bool healthy() const override; + + // momentary switch to change camera between picture and video modes + void cam_mode_toggle() override; + + // entry point to actually take a picture. returns true on success + bool trigger_pic() override; + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const override; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + // initialize the RunCam driver - void init(); + void init() override; // camera button simulation bool simulate_camera_button(const ControlOperation operation, const uint32_t transition_timeout = RUNCAM_DEFAULT_BUTTON_PRESS_DELAY); // start the video void start_recording(); // stop the video void stop_recording(); + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool _start_recording) override { + if (_start_recording) { + start_recording(); + } else { + stop_recording(); + } + return true; + } + // enter the OSD menu void enter_osd(); // exit the OSD menu void exit_osd(); // OSD control determined by camera options void osd_option(); - // update loop - void update(); + // update - should be called at 50hz + void update() override; // Check whether arming is allowed bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; @@ -265,6 +294,10 @@ private: static uint8_t _sub_menu_lengths[RUNCAM_NUM_SUB_MENUS]; // shared inbound scratch space uint8_t _recv_buf[RUNCAM_MAX_PACKET_SIZE]; // all the response contexts use same recv buffer + // the runcam instance + uint8_t _runcam_instance; + + static const char* _models[RUNCAM_MAX_DEVICE_TYPES]; class Request; @@ -435,4 +468,4 @@ namespace AP AP_RunCam *runcam(); }; -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED From 631c4944ff4d9b167900c111b57315d5a883d806 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Dec 2024 08:50:47 +0000 Subject: [PATCH 08/26] scripts: correct runcam build option description --- Tools/scripts/build_options.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 659d8c5cd4..ae83948aef 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -146,7 +146,7 @@ BUILD_OPTIONS = [ Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable GCS camera thermal range', 0, 'Camera,MOUNT'), # noqa: E501 Feature('Camera', 'Camera_Info_From_Script', 'AP_CAMERA_INFO_FROM_SCRIPT_ENABLED', 'Enable Camera information messages via Lua script', 0, 'Camera,SCRIPTING'), # noqa - Feature('Camera', 'RUNCAM', 'AP_CAMERA_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None), + Feature('Camera', 'RUNCAM', 'AP_CAMERA_RUNCAM_ENABLED', 'Enable RunCam control', 0, None), Feature('Copter', 'MODE_ZIGZAG', 'MODE_ZIGZAG_ENABLED', 'Enable Mode ZigZag', 0, None), Feature('Copter', 'MODE_SYSTEMID', 'MODE_SYSTEMID_ENABLED', 'Enable Mode SystemID', 0, 'Logging'), From 684d95a8260f2d9adf694a06873911f90e0e4b6c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 13 Dec 2024 08:44:09 +0000 Subject: [PATCH 09/26] AP_Camera: remove lf from panic correct camera info message and defend against -1 --- libraries/AP_Camera/AP_RunCam.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp index c6d461667b..ce089e694b 100644 --- a/libraries/AP_Camera/AP_RunCam.cpp +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -131,7 +131,7 @@ AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t inst { AP_Param::setup_object_defaults(this, var_info); if (_singleton != nullptr && _singleton->_instance == instance) { - AP_HAL::panic("AP_RunCam instance must be a singleton %u\n", instance); + AP_HAL::panic("AP_RunCam instance must be a singleton %u", instance); } if (_singleton == nullptr) { _singleton = this; @@ -1078,12 +1078,14 @@ bool AP_RunCam::healthy() const } // momentary switch to change camera between picture and video modes -void AP_RunCam::cam_mode_toggle() { +void AP_RunCam::cam_mode_toggle() +{ } // entry point to actually take a picture. returns true on success -bool AP_RunCam::trigger_pic() { +bool AP_RunCam::trigger_pic() +{ return false; } @@ -1091,7 +1093,7 @@ bool AP_RunCam::trigger_pic() { void AP_RunCam::send_camera_information(mavlink_channel_t chan) const { // exit immediately if not initialised - if (!camera_ready() || _cam_type.get() == 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) { + if (!camera_ready() || _cam_type.get() <= 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) { return; } @@ -1118,9 +1120,9 @@ void AP_RunCam::send_camera_information(mavlink_channel_t chan) const vendor_name, // vendor_name uint8_t[32] model_name, // model_name uint8_t[32] 0, // firmware version uint32_t + NaNf, // focal_length float (mm) NaNf, // sensor_size_h float (mm) NaNf, // sensor_size_v float (mm) - 0, // sensor_size_v float (mm) 0, // resolution_h uint16_t (pix) 0, // resolution_v uint16_t (pix) 0, // lens_id uint8_t From d68b95e60e259871f18b5e5a95d9f138fde5c395 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:43 +1100 Subject: [PATCH 10/26] AP_AHRS: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_AHRS/AP_AHRS_View.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 450567ce2c..a2bfa34d1d 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -36,7 +36,7 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_ y_angle = 270; break; default: - AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation); + AP_HAL::panic("Unsupported AHRS view %u", (unsigned)rotation); } _pitch_trim_deg = pitch_trim_deg; From f59efb8a19322f2b8a67db9150520f244dc9c55a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:43 +1100 Subject: [PATCH 11/26] AP_DDS: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_DDS/AP_DDS_Client.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 0a7a81689b..e5404898d9 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1452,7 +1452,7 @@ void AP_DDS_Client::write_time_topic() const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: XRCE_Client failed to serialize"); } } } @@ -1468,7 +1468,7 @@ void AP_DDS_Client::write_nav_sat_fix_topic() const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1485,7 +1485,7 @@ void AP_DDS_Client::write_static_transforms() const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1502,7 +1502,7 @@ void AP_DDS_Client::write_battery_state_topic() const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1519,7 +1519,7 @@ void AP_DDS_Client::write_local_pose_topic() const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1536,7 +1536,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic() const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1552,7 +1552,7 @@ void AP_DDS_Client::write_tx_local_airspeed_topic() const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1568,7 +1568,7 @@ void AP_DDS_Client::write_imu_topic() const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1585,7 +1585,7 @@ void AP_DDS_Client::write_geo_pose_topic() const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1602,7 +1602,7 @@ void AP_DDS_Client::write_clock_topic() const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1618,7 +1618,7 @@ void AP_DDS_Client::write_gps_global_origin_topic() uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size); const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic); if (!success) { - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1634,7 +1634,7 @@ void AP_DDS_Client::write_goal_topic() uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size); const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic); if (!success) { - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1651,7 +1651,7 @@ void AP_DDS_Client::write_status_topic() const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } From 14b29cf7dee10729577677e065b2376c4c50dbba Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 12/26] AP_FlashStorage: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- .../examples/FlashTest/FlashTest.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp index f617b4850a..fe76f28429 100644 --- a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp +++ b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp @@ -46,17 +46,17 @@ private: bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) { if (sector > 1) { - AP_HAL::panic("FATAL: write to sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: write to sector %u", (unsigned)sector); } if (offset + length > flash_sector_size) { - AP_HAL::panic("FATAL: write to sector %u at offset %u length %u\n", + AP_HAL::panic("FATAL: write to sector %u at offset %u length %u", (unsigned)sector, (unsigned)offset, (unsigned)length); } uint8_t *b = &flash[sector][offset]; if ((offset & 1) || (length & 1)) { - AP_HAL::panic("FATAL: invalid write at %u:%u len=%u\n", + AP_HAL::panic("FATAL: invalid write at %u:%u len=%u", (unsigned)sector, (unsigned)offset, (unsigned)length); @@ -66,7 +66,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data const uint16_t v = le16toh_ptr(&data[i*2]); uint16_t v2 = le16toh_ptr(&b[i*2]); if (v & !v2) { - AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n", + AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x", (unsigned)sector, unsigned(offset+i), b[i], @@ -74,7 +74,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data } #ifndef AP_FLASHSTORAGE_MULTI_WRITE if (v != v2 && v != 0xFFFF && v2 != 0xFFFF) { - AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n", + AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x", (unsigned)sector, unsigned(offset+i), b[i], @@ -90,10 +90,10 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) { if (sector > 1) { - AP_HAL::panic("FATAL: read from sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: read from sector %u", (unsigned)sector); } if (offset + length > flash_sector_size) { - AP_HAL::panic("FATAL: read from sector %u at offset %u length %u\n", + AP_HAL::panic("FATAL: read from sector %u at offset %u length %u", (unsigned)sector, (unsigned)offset, (unsigned)length); @@ -105,7 +105,7 @@ bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint1 bool FlashTest::flash_erase(uint8_t sector) { if (sector > 1) { - AP_HAL::panic("FATAL: erase sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: erase sector %u", (unsigned)sector); } memset(&flash[sector][0], 0xFF, flash_sector_size); return true; From e6796c1e9659fbc78cb3d8a23d5fcc5ba9c5a4ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 13/26] AP_HAL_ChibiOS: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index b8c995a4b2..4bc97e1414 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -191,7 +191,7 @@ void UARTDriver::thread_rx_init(void) uart_rx_thread, nullptr); if (uart_rx_thread_ctx == nullptr) { - AP_HAL::panic("Could not create UART RX thread\n"); + AP_HAL::panic("Could not create UART RX thread"); } } } @@ -209,7 +209,7 @@ void UARTDriver::thread_init(void) uart_thread_trampoline, this); if (uart_thread_ctx == nullptr) { - AP_HAL::panic("Could not create UART TX thread\n"); + AP_HAL::panic("Could not create UART TX thread"); } } } From dc2a5e02b6f346417a63460d2ffa666e38829d63 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 14/26] AP_HAL_Linux: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp | 2 +- libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp | 4 ++-- libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp | 10 +++++----- libraries/AP_HAL_Linux/RCInput_Navio2.cpp | 4 ++-- libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_PRU.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp | 2 +- libraries/AP_HAL_Linux/SPIUARTDriver.cpp | 2 +- libraries/AP_HAL_Linux/VideoIn.cpp | 2 +- 9 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp b/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp index faf450ee98..866c23a990 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp @@ -20,7 +20,7 @@ void AnalogSource_Navio2::set_channel(uint8_t pin) } if (asprintf(&channel_path, "%s/ch%d", ADC_BASE_PATH, pin) == -1) { - AP_HAL::panic("asprintf failed\n"); + AP_HAL::panic("asprintf failed"); } if (_fd >= 0) { diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp index ba53789c8a..cc512f53e5 100644 --- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp +++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp @@ -126,7 +126,7 @@ CameraSensor_Mt9v117::CameraSensor_Mt9v117(const char *device_path, _configure_sensor_qvga(); break; default: - AP_HAL::panic("mt9v117: unsupported resolution\n"); + AP_HAL::panic("mt9v117: unsupported resolution"); break; } @@ -409,7 +409,7 @@ void CameraSensor_Mt9v117::_init_sensor() id = _read_reg16(CHIP_ID); if (id != MT9V117_CHIP_ID) { - AP_HAL::panic("Mt9v117: bad chip id 0x%04x\n", id); + AP_HAL::panic("Mt9v117: bad chip id 0x%04x", id); } _soft_reset(); _apply_patch(); diff --git a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp index 2429449950..a7817371eb 100644 --- a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp +++ b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp @@ -88,7 +88,7 @@ void OpticalFlow_Onboard::init() if (!_camerasensor->set_format(HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH, HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT, V4L2_MBUS_FMT_UYVY8_2X8)) { - AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt"); } _format = V4L2_PIX_FMT_NV12; #endif @@ -100,7 +100,7 @@ void OpticalFlow_Onboard::init() if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY && _format != V4L2_PIX_FMT_YUYV) { - AP_HAL::panic("OpticalFlow_Onboard: format not supported\n"); + AP_HAL::panic("OpticalFlow_Onboard: format not supported"); } if (_width == HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH && @@ -272,7 +272,7 @@ void OpticalFlow_Onboard::_run_optflow() convert_buffer = (uint8_t *)calloc(1, convert_buffer_size); if (!convert_buffer) { - AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer"); } } @@ -286,7 +286,7 @@ void OpticalFlow_Onboard::_run_optflow() free(convert_buffer); } - AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer"); } } @@ -322,7 +322,7 @@ void OpticalFlow_Onboard::_run_optflow() free(output_buffer); } - AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame"); } if (_format == V4L2_PIX_FMT_YUYV) { diff --git a/libraries/AP_HAL_Linux/RCInput_Navio2.cpp b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp index 225976ea0b..4fb9e23270 100644 --- a/libraries/AP_HAL_Linux/RCInput_Navio2.cpp +++ b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp @@ -23,7 +23,7 @@ void RCInput_Navio2::init() for (size_t i = 0; i < ARRAY_SIZE(channels); i++) { channels[i] = open_channel(i); if (channels[i] < 0) { - AP_HAL::panic("[RCInput_Navio2]: failed to open channels\n"); + AP_HAL::panic("[RCInput_Navio2]: failed to open channels"); } } } @@ -64,7 +64,7 @@ int RCInput_Navio2::open_channel(int channel) { char *channel_path; if (asprintf(&channel_path, "%s/ch%d", RCIN_SYSFS_PATH, channel) == -1) { - AP_HAL::panic("[RCInput_Navio2]: not enough memory\n"); + AP_HAL::panic("[RCInput_Navio2]: not enough memory"); } int fd = ::open(channel_path, O_RDONLY|O_CLOEXEC); diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp index c141f40d62..999f25328a 100644 --- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp @@ -32,7 +32,7 @@ using namespace Linux; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutputAioPRU.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutputAioPRU.cpp:SIGBUS error generated"); } void RCOutput_AioPRU::init() { diff --git a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp index b20ae3071b..a36b9fac4a 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp @@ -23,7 +23,7 @@ static const uint8_t chan_pru_map[]= {10,8,11,9,7,6,5,4,3,2,1,0}; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutput.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error generated"); } void RCOutput_PRU::init() { diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp index f64273e964..b2e8d7b483 100644 --- a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp @@ -31,7 +31,7 @@ using namespace Linux; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutput.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error generated"); } void RCOutput_ZYNQ::init() { diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index e88b388b34..b2f705e4b4 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -57,7 +57,7 @@ void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) _buffer = NEW_NOTHROW uint8_t[rxS]; if (_buffer == nullptr) { hal.console->printf("Not enough memory\n"); - AP_HAL::panic("Not enough memory\n"); + AP_HAL::panic("Not enough memory"); } } diff --git a/libraries/AP_HAL_Linux/VideoIn.cpp b/libraries/AP_HAL_Linux/VideoIn.cpp index 1e251d7d4b..d685181d6f 100644 --- a/libraries/AP_HAL_Linux/VideoIn.cpp +++ b/libraries/AP_HAL_Linux/VideoIn.cpp @@ -45,7 +45,7 @@ bool VideoIn::get_frame(Frame &frame) { if (!_streaming) { if (!_set_streaming(true)) { - AP_HAL::panic("couldn't start streaming\n"); + AP_HAL::panic("couldn't start streaming"); return false; } _streaming = true; From 57ac1e591101d513c1427230a4b6acd008c2618e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 15/26] AP_HAL_SITL: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_HAL_SITL/Scheduler.cpp | 2 +- libraries/AP_HAL_SITL/Storage.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index ccc40042e8..329617d877 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -414,7 +414,7 @@ void Scheduler::check_thread_stacks(void) const uint8_t ncheck = 8; for (uint8_t i=0; istack_min[i] != stackfill) { - AP_HAL::panic("stack overflow in thread %s\n", p->name); + AP_HAL::panic("stack overflow in thread %s", p->name); } } } diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index 98e28e5f99..c82f8351f2 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -279,7 +279,7 @@ static bool sitl_flash_write(uint32_t addr, const uint8_t *data, uint32_t length } #if defined(HAL_FLASH_MIN_WRITE_SIZE) && HAL_FLASH_MIN_WRITE_SIZE == 32 if ((length % HAL_FLASH_MIN_WRITE_SIZE) != 0 || (addr % HAL_FLASH_MIN_WRITE_SIZE) != 0) { - AP_HAL::panic("Attempt to write flash at %u length %u\n", addr, length); + AP_HAL::panic("Attempt to write flash at %u length %u", addr, length); } // emulate H7 requirement that writes be to untouched bytes for (uint32_t i=0; i Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 16/26] AP_InertialSensor: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp | 6 +++--- libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp | 4 ++-- libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 777ab3cf0e..aaa7756d08 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -226,10 +226,10 @@ void AP_InertialSensor_BMI160::_check_err_reg() r = _dev->read_registers(BMI160_REG_ERR_REG, &v, 1); if (!r) { - AP_HAL::panic("BMI160: couldn't read ERR_REG\n"); + AP_HAL::panic("BMI160: couldn't read ERR_REG"); } if (v) { - AP_HAL::panic("BMI160: error detected on ERR_REG\n"); + AP_HAL::panic("BMI160: error detected on ERR_REG"); } #endif } @@ -514,4 +514,4 @@ bool AP_InertialSensor_BMI160::_init() } return ret; -} \ No newline at end of file +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index a62fd1eb2f..08c6d41792 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -424,7 +424,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor() if (_drdy_pin_num_a >= 0) { _drdy_pin_a = hal.gpio->channel(_drdy_pin_num_a); if (_drdy_pin_a == nullptr) { - AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel"); } _drdy_pin_a->mode(HAL_GPIO_INPUT); @@ -433,7 +433,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor() if (_drdy_pin_num_g >= 0) { _drdy_pin_g = hal.gpio->channel(_drdy_pin_num_g); if (_drdy_pin_g == nullptr) { - AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel"); } _drdy_pin_g->mode(HAL_GPIO_INPUT); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index d95cfce0f0..beef216fc2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -231,7 +231,7 @@ bool AP_InertialSensor_LSM9DS1::_init_sensor() if (_drdy_pin_num_xg >= 0) { _drdy_pin_xg = hal.gpio->channel(_drdy_pin_num_xg); if (_drdy_pin_xg == nullptr) { - AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel"); } _drdy_pin_xg->mode(HAL_GPIO_INPUT); } From 7fe183ba0ff5f0411ee3788b0ca696c132ee9066 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 17/26] AP_InternalError: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_InternalError/AP_InternalError.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InternalError/AP_InternalError.cpp b/libraries/AP_InternalError/AP_InternalError.cpp index 0f23b2d1d4..c1319b1126 100644 --- a/libraries/AP_InternalError/AP_InternalError.cpp +++ b/libraries/AP_InternalError/AP_InternalError.cpp @@ -127,7 +127,7 @@ void AP_stack_overflow(const char *thread_name) } hal.util->persistent_data.fault_type = 42; // magic value if (!hal.util->get_soft_armed()) { - AP_HAL::panic("stack overflow %s\n", thread_name); + AP_HAL::panic("stack overflow %s", thread_name); } } @@ -137,7 +137,7 @@ void AP_memory_guard_error(uint32_t size) INTERNAL_ERROR(AP_InternalError::error_t::mem_guard); if (!hal.util->get_soft_armed()) { ::printf("memory guard error size=%u\n", unsigned(size)); - AP_HAL::panic("memory guard size=%u\n", unsigned(size)); + AP_HAL::panic("memory guard size=%u", unsigned(size)); } } From 074798fb8898121173e78381c9185d151037680e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:44 +1100 Subject: [PATCH 18/26] AP_Logger: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_Logger/AP_Logger_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 6d9db1a43b..e2f18ef6fe 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -403,7 +403,7 @@ void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer, } else { strncpy(name, "?NM?", ARRAY_SIZE(name)); } - AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)\n", + AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)", type, name, type_len, size); } } From 70e9a90ac5999507a6b648a03e599e3b713072fd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:45 +1100 Subject: [PATCH 19/26] AP_Param: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_Param/AP_Param.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 9962780398..d23a10e085 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1616,7 +1616,7 @@ void AP_Param::load_defaults_file_from_filesystem(const char *default_file, bool #endif } else { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - AP_HAL::panic("Failed to load defaults from %s\n", default_file); + AP_HAL::panic("Failed to load defaults from %s", default_file); #else printf("Failed to load defaults from %s\n", default_file); #endif From d540a31f2effdb62db09bdbc540eff2a4e1cf01d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:45 +1100 Subject: [PATCH 20/26] AP_Radio: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_Radio/AP_Radio_bk2425.cpp | 2 +- libraries/AP_Radio/AP_Radio_cc2500.cpp | 2 +- libraries/AP_Radio/AP_Radio_cypress.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Radio/AP_Radio_bk2425.cpp b/libraries/AP_Radio/AP_Radio_bk2425.cpp index 773205cf88..d9dd185d6a 100644 --- a/libraries/AP_Radio/AP_Radio_bk2425.cpp +++ b/libraries/AP_Radio/AP_Radio_bk2425.cpp @@ -140,7 +140,7 @@ AP_Radio_beken::AP_Radio_beken(AP_Radio &_radio) : bool AP_Radio_beken::init(void) { if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_beken: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_beken: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index cce9b4ffcf..83ee84f9ce 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -85,7 +85,7 @@ bool AP_Radio_cc2500::init(void) { #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_cc2500: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_cc2500: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index fc27088469..43bebe982c 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -264,7 +264,7 @@ bool AP_Radio_cypress::init(void) dev = hal.spi->get_device(CYRF_SPI_DEVICE); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, From 6fb3e27b8d5dda9f656828636edf365202775eb6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:45 +1100 Subject: [PATCH 21/26] AP_RCProtocol: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp index f0f7cafdea..2ceb6c0ed6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp @@ -45,7 +45,7 @@ AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtoc { #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) if (_singleton != nullptr) { - AP_HAL::panic("Duplicate SRXL2 handler\n"); + AP_HAL::panic("Duplicate SRXL2 handler"); } _singleton = this; @@ -64,12 +64,12 @@ void AP_RCProtocol_SRXL2::_bootstrap(uint8_t device_id) // Init the local SRXL device if (!srxlInitDevice(device_id, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, device_id)) { - AP_HAL::panic("Failed to initialize SRXL2 device\n"); + AP_HAL::panic("Failed to initialize SRXL2 device"); } // Init the SRXL bus: The bus index must always be < SRXL_NUM_OF_BUSES -- in this case, it can only be 0 if (!srxlInitBus(0, 0, SRXL_SUPPORTED_BAUD_RATES)) { - AP_HAL::panic("Failed to initialize SRXL2 bus\n"); + AP_HAL::panic("Failed to initialize SRXL2 bus"); } _device_id = device_id; From fea17e6f594d73756aac2d14c1dfad62f2456ce3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:45 +1100 Subject: [PATCH 22/26] SITL: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- libraries/SITL/SIM_Frame.cpp | 6 +++--- libraries/SITL/SIM_JSON_Master.cpp | 6 +++--- libraries/SITL/SIM_XPlane.cpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index d5435047f9..c3f2fba101 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -338,15 +338,15 @@ void Frame::load_frame_params(const char *model_json) } else { IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", model_json)); if (AP::FS().stat(model_json, &st) != 0) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } } if (fname == nullptr) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } AP_JSON::value *obj = AP_JSON::load_json(model_json); if (obj == nullptr) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } enum class VarType { diff --git a/libraries/SITL/SIM_JSON_Master.cpp b/libraries/SITL/SIM_JSON_Master.cpp index 9b8c3b5906..a6b92591d4 100644 --- a/libraries/SITL/SIM_JSON_Master.cpp +++ b/libraries/SITL/SIM_JSON_Master.cpp @@ -37,15 +37,15 @@ void JSON_Master::init(const int32_t num_slaves) uint16_t port = 9002 + 10 * i; if (!list->sock_in.reuseaddress()) { - AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno)); } if (!list->sock_in.bind("127.0.0.1", port)) { - AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno)); } if (!list->sock_in.set_blocking(false)) { - AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s", port, strerror(errno)); } printf("Slave %u: listening on %u\n", list->instance, port); diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index 3633bb4b1a..e9c147c633 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -116,7 +116,7 @@ XPlane::XPlane(const char *frame_str) : #endif if (!load_dref_map(XPLANE_JSON)) { - AP_HAL::panic("%s failed to load\n", XPLANE_JSON); + AP_HAL::panic("%s failed to load", XPLANE_JSON); } } From b5ba09664e1a249773aae83fe873dfdffeaf2ca6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 13 Dec 2024 20:15:45 +1100 Subject: [PATCH 23/26] Tools: remove superfluous linefeed from panic strings panic adds this within the HAL layer. --- Tools/CPUInfo/CPUInfo.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index cf3e43b1b0..e598c97724 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -212,13 +212,13 @@ static void test_div1000(void) for (uint32_t i=0; i<2000000; i++) { uint64_t v = 0; if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) { - AP_HAL::panic("ERROR: div1000 no random\n"); + AP_HAL::panic("ERROR: div1000 no random"); break; } uint64_t v1 = v / 1000ULL; uint64_t v2 = uint64_div1000(v); if (v1 != v2) { - AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n", + AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx", (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); return; } @@ -228,7 +228,7 @@ static void test_div1000(void) for (uint32_t i=0; i<2000000; i++) { uint64_t v = 0; if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) { - AP_HAL::panic("ERROR: div1000 no random\n"); + AP_HAL::panic("ERROR: div1000 no random"); break; } chSysLock(); @@ -236,7 +236,7 @@ static void test_div1000(void) uint64_t v2 = uint64_div1000(v); chSysUnlock(); if (v1 != v2) { - AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n", + AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx", (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); return; } From 1ab0dcd45d47826c40a2d63d513c484d51dadf5b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 12:51:47 +1100 Subject: [PATCH 24/26] AP_TECS: remove unneccessary sqrtf in Phi calculations we take the square of this in the only use of it --- libraries/AP_TECS/AP_TECS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 69575aab7a..d75faf92fa 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -753,8 +753,8 @@ void AP_TECS::_update_throttle_with_airspeed(void) // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. - const float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); - STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); + const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); + STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); const float ff_throttle = nomThr + STEdot_dem / K_thr2STE; // Calculate PD + FF throttle @@ -907,8 +907,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. - float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); - float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); + const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); + float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); constrain_throttle(); From 9835fe80d6fc4d77e2e99d956f3fee3476d17593 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 22:35:28 +1100 Subject: [PATCH 25/26] AP_TECS: correct comment explaining roll-induced-drag scaling Co-authored-by: George Zogopoulos --- libraries/AP_TECS/AP_TECS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index d75faf92fa..b0cbe34087 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -751,7 +751,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) const float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); @@ -905,7 +905,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi // Calculate additional throttle for turn drag compensation including throttle nudging const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); From eedb8ae6178e85426db478528205d99ed019782c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 24 Nov 2024 13:30:34 +1100 Subject: [PATCH 26/26] hwdef: remove 'HAL_WATCHDOG_ENABLED_DEFAULT true' from periphs this is the default for peripherals --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat | 3 --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat | 3 --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat | 3 --- libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat | 3 --- 4 files changed, 12 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat index 46b4ac476a..48921930f7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat @@ -19,9 +19,6 @@ env AP_PERIPH 1 define STM32_ST_USE_TIMER 5 define CH_CFG_ST_RESOLUTION 32 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat index 840e642e23..ec933521d9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat @@ -20,9 +20,6 @@ env AP_PERIPH 1 STM32_ST_USE_TIMER 5 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat index 726e786945..c70e2a7425 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat @@ -20,9 +20,6 @@ env AP_PERIPH 1 STM32_ST_USE_TIMER 5 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat index 5345cd6617..82c206bfa6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat @@ -18,9 +18,6 @@ define HAL_STORAGE_SIZE 800 # setup build for a peripheral firmware env AP_PERIPH 1 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 8000000