From dc357a0d57033f56b81c03c9a6026f8b2d8f0054 Mon Sep 17 00:00:00 2001 From: latercomer Date: Mon, 29 Apr 2024 23:15:29 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=B9=E9=87=8F=E5=A4=84=E7=90=86=E5=A4=B4?= =?UTF-8?q?=E6=96=87=E4=BB=B6=EF=BC=8C=E5=8E=BB=E6=8E=89=E4=B8=8D=E6=94=AF?= =?UTF-8?q?=E6=8C=81=E7=9A=84=E8=B7=AF=E5=BE=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ArmAuthorization/ArmAuthorization.cpp | 5 +- apps/controller/commander/Commander.cpp | 46 +- .../commander/accelerometer_calibration.cpp | 2 - .../commander/airspeed_calibration.cpp | 228 ++++--- .../controller/commander/baro_calibration.cpp | 3 +- .../commander/calibration_routines.cpp | 3 +- .../controller/commander/commander_helper.cpp | 1 - apps/controller/commander/esc_calibration.cpp | 2 - .../failure_detector_params.c | 1 - .../controller/commander/gyro_calibration.cpp | 4 - .../commander/level_calibration.cpp | 3 +- apps/controller/commander/mag_calibration.cpp | 2 - apps/controller/commander/rc_calibration.cpp | 2 - apps/controller/commander/worker_thread.cpp | 2 +- apps/controller/commander/worker_thread.hpp | 1 - .../FixedwingAttitudeControl.hpp | 7 +- .../fw_autotune_attitude_control.hpp | 4 +- .../FixedwingPositionControl.hpp | 13 +- .../fw_rate_control/FixedwingRateControl.hpp | 3 - apps/controller/land_detector/LandDetector.h | 9 +- .../land_detector/land_detector_main.cpp | 7 +- .../LandingTargetEstimator.cpp | 1 - .../LandingTargetEstimator.h | 3 +- .../landing_target_estimator_main.cpp | 5 +- .../mc_autotune_attitude_control.hpp | 6 +- .../MulticopterPositionControl.hpp | 12 +- apps/controller/rc_update/rc_update.h | 5 +- .../vtol_att_control/vtol_att_control_main.h | 29 +- .../attitude_estimator_q_main.cpp | 49 +- apps/estimator/ekf2/EKF2.hpp | 154 ++--- apps/estimator/ekf2/EKF2Selector.hpp | 43 +- .../gyro_calibration/GyroCalibration.cpp | 26 +- .../gyro_calibration/GyroCalibration.hpp | 8 +- apps/estimator/gyro_fft/GyroFFT.hpp | 7 +- .../BlockLocalPositionEstimator.cpp | 83 ++- .../BlockLocalPositionEstimator.hpp | 135 ++-- .../local_position_estimator/SConscript | 2 +- .../mag_bias_estimator/MagBiasEstimator.cpp | 8 +- .../mag_bias_estimator/MagBiasEstimator.hpp | 7 +- .../sensors/data_validator/SConscript | 2 +- .../VehicleAcceleration.hpp | 1 - .../vehicle_air_data/VehicleAirData.hpp | 1 - .../VehicleAngularVelocity.hpp | 1 - .../VehicleGPSPosition.hpp | 1 - apps/estimator/sensors/vehicle_imu/SConscript | 2 +- .../sensors/vehicle_imu/VehicleIMU.hpp | 1 - .../VehicleMagnetometer.hpp | 1 - .../VehicleOpticalFlow.hpp | 1 - .../temperature_compensation/SConscript | 2 +- .../TemperatureCompensationModule.cpp | 33 +- .../TemperatureCompensationModule.h | 10 +- .../temperature_calibration/accel.cpp | 2 +- .../temperature_calibration/accel.h | 2 +- .../temperature_calibration/baro.cpp | 2 +- .../temperature_calibration/baro.h | 2 +- .../temperature_calibration/common.h | 20 +- .../temperature_calibration/gyro.cpp | 4 +- .../temperature_calibration/gyro.h | 5 +- .../temperature_calibration/polyfit.hpp | 15 +- .../temperature_calibration/task.cpp | 11 +- .../console_buffer/console_buffer_usr.cpp | 4 +- .../common/mathlib/math/WelfordMeanVector.hpp | 2 + apps/libraries/controller/adsb/AdsbConflict.h | 1 - apps/libraries/controller/rc/dsm.cpp | 4 +- apps/libraries/controller/rc/dsm.h | 2 +- apps/libraries/controller/rc/sbus.cpp | 2 +- .../sensor_calibration/Accelerometer.hpp | 2 +- .../sensor_calibration/Barometer.hpp | 2 +- .../sensor_calibration/Gyroscope.hpp | 1 - .../sensor_calibration/Magnetometer.hpp | 2 +- .../peripheral/battery/battery/battery.h | 1 - .../peripheral/battery/smbus_sbs/SBS.hpp | 2 +- apps/libraries/peripheral/drivers/drv_hrt.h | 3 +- apps/libraries/peripheral/drivers/led/led.cpp | 1 - apps/libraries/peripheral/rcinput/rc/crsf.cpp | 2 +- apps/libraries/peripheral/rcinput/rc/crsf.h | 2 +- apps/libraries/peripheral/rcinput/rc/dsm.cpp | 8 +- apps/libraries/peripheral/rcinput/rc/dsm.h | 4 +- apps/libraries/peripheral/rcinput/rc/ghst.cpp | 2 +- apps/libraries/peripheral/rcinput/rc/ghst.hpp | 2 +- .../peripheral/rcinput/rc/rc_tests/RCTest.cpp | 15 +- apps/libraries/peripheral/rcinput/rc/sbus.cpp | 5 +- .../telemetry/button/ButtonPublisher.hpp | 2 +- .../libraries/telemetry/timesync/Timesync.hpp | 2 +- .../battery/battery_status/analog_battery.cpp | 13 +- .../battery/battery_status/battery_status.cpp | 45 +- .../battery/esc_battery/EscBattery.hpp | 9 +- .../camera_feedback/CameraFeedback.hpp | 17 +- apps/peripheral/payload/gimbal/gimbal.cpp | 43 +- .../peripheral/payload/gimbal/gimbal_params.h | 2 +- .../payload/gimbal/input_mavlink.cpp | 55 +- .../peripheral/payload/gimbal/input_mavlink.h | 2 +- apps/peripheral/payload/gimbal/input_rc.cpp | 6 +- apps/peripheral/payload/gimbal/input_test.cpp | 8 +- apps/peripheral/payload/gimbal/input_test.h | 2 +- apps/peripheral/payload/gimbal/output.cpp | 8 +- apps/peripheral/payload/gimbal/output.h | 4 +- .../payload/gimbal/output_mavlink.cpp | 8 +- apps/peripheral/payload/gimbal/output_rc.cpp | 16 +- .../payload/payload_deliverer/gripper.h | 2 +- .../payload_deliverer/payload_deliverer.h | 9 +- apps/simulation/gz_bridge/GZBridge.hpp | 3 +- .../simulator_mavlink/SimulatorMavlink.cpp | 42 +- .../simulator_mavlink/SimulatorMavlink.hpp | 8 +- apps/storage/logger/log_writer_file.cpp | 2 +- apps/storage/logger/watchdog.cpp | 2 +- apps/storage/replay/Replay.cpp | 37 +- apps/storage/replay/Replay.hpp | 4 +- apps/storage/replay/ReplayEkf2.cpp | 8 +- apps/telemetry/mavlink/mavlink.c | 2 +- .../mavlink/mavlink_command_sender.cpp | 2 +- .../mavlink/mavlink_command_sender.h | 5 +- apps/telemetry/mavlink/mavlink_events.h | 6 +- apps/telemetry/mavlink/mavlink_ftp.h | 4 +- apps/telemetry/mavlink/mavlink_log_handler.h | 2 +- apps/telemetry/mavlink/mavlink_main.cpp | 12 +- apps/telemetry/mavlink/mavlink_main.h | 14 +- apps/telemetry/mavlink/mavlink_messages.cpp | 10 +- apps/telemetry/mavlink/mavlink_mission.cpp | 55 +- apps/telemetry/mavlink/mavlink_parameters.cpp | 48 +- apps/telemetry/mavlink/mavlink_parameters.h | 3 +- apps/telemetry/mavlink/mavlink_rate_limiter.h | 2 +- apps/telemetry/mavlink/mavlink_receiver.cpp | 8 +- apps/telemetry/mavlink/mavlink_receiver.h | 10 +- apps/telemetry/mavlink/mavlink_shell.cpp | 12 +- apps/telemetry/mavlink/mavlink_shell.h | 4 +- .../mavlink/mavlink_simple_analyzer.cpp | 4 +- apps/telemetry/mavlink/mavlink_stream.h | 4 +- apps/telemetry/mavlink/mavlink_timesync.h | 2 +- apps/telemetry/mavlink/mavlink_ulog.cpp | 2 +- apps/telemetry/mavlink/mavlink_ulog.h | 7 +- .../mavlink/streams/COMPONENT_INFORMATION.hpp | 7 +- .../mavlink/streams/COMPONENT_METADATA.hpp | 4 +- .../mavlink/streams/HIGH_LATENCY2.hpp | 38 +- .../mavlink/streams/MAG_CAL_REPORT.hpp | 5 +- apps/telemetry/mavlink/streams/SCALED_IMU.hpp | 6 +- .../telemetry/mavlink/streams/SCALED_IMU2.hpp | 6 +- .../telemetry/mavlink/streams/SCALED_IMU3.hpp | 7 +- apps/telemetry/mavlink/streams/STATUSTEXT.hpp | 4 +- apps/telemetry/mavlink/streams/SYS_STATUS.hpp | 15 +- apps/telemetry/mavlink/streams/TIMESYNC.hpp | 4 +- apps/telemetry/mavlink/timestamped_list.h | 2 +- apps/telemetry/mavlink/tune_publisher.cpp | 2 +- apps/telemetry/mavlink/tune_publisher.h | 4 +- apps/telemetry/uavcan/actuators/esc.cpp | 14 +- apps/telemetry/uavcan/actuators/esc.hpp | 6 +- apps/telemetry/uavcan/actuators/servo.cpp | 2 +- apps/telemetry/uavcan/actuators/servo.hpp | 6 +- apps/telemetry/uavcan/beep.cpp | 2 +- apps/telemetry/uavcan/beep.hpp | 2 +- apps/telemetry/uavcan/logmessage.hpp | 3 +- apps/telemetry/uavcan/rgbled.hpp | 4 +- apps/telemetry/uavcan/sensors/accel.cpp | 2 +- apps/telemetry/uavcan/sensors/airspeed.cpp | 5 +- apps/telemetry/uavcan/sensors/baro.cpp | 4 +- apps/telemetry/uavcan/sensors/battery.cpp | 19 +- apps/telemetry/uavcan/sensors/battery.hpp | 4 +- .../uavcan/sensors/differential_pressure.cpp | 4 +- apps/telemetry/uavcan/sensors/flow.cpp | 2 +- apps/telemetry/uavcan/sensors/gnss.cpp | 15 +- apps/telemetry/uavcan/sensors/gnss.hpp | 6 +- apps/telemetry/uavcan/sensors/gyro.cpp | 2 +- apps/telemetry/uavcan/sensors/hygrometer.cpp | 4 +- apps/telemetry/uavcan/sensors/mag.cpp | 4 +- apps/telemetry/uavcan/sensors/rangefinder.cpp | 2 +- .../uavcan/sensors/safety_button.cpp | 2 +- .../uavcan/sensors/sensor_bridge.hpp | 4 +- .../socketcan/driver/src/clock.cpp | 2 +- .../socketcan/driver/src/socketcan.cpp | 2 +- apps/telemetry/uavcan/uavcan_main.cpp | 28 +- apps/telemetry/uavcan/uavcan_main.hpp | 37 +- apps/telemetry/uavcan/uavcan_module.hpp | 1 - apps/telemetry/uavcan/uavcan_servers.cpp | 6 +- apps/telemetry/uavcan/uavcan_servers.hpp | 1 - .../Micro-XRCE-DDS-Client/src/c/util/time.c | 2 +- .../uxrce_dds_client/uxrce_dds_client.cpp | 143 +++-- .../uxrce_dds_client/uxrce_dds_client.h | 6 +- pkgs/board_identity/board_identity.c | 1 - pkgs/board_identity/board_identity_test.cpp | 1 - pkgs/board_identity/stm32/board_identity.c | 1 - pkgs/board_identity/stm32/board_mcu_version.c | 1 - pkgs/crypto/crypto.h | 124 ++-- pkgs/device/cdev/posix/cdev_platform.cpp | 5 +- pkgs/device/cdev/test/cdevtest_example.cpp | 6 +- pkgs/device/cdev/test/cdevtest_start.cpp | 2 +- pkgs/device/drv-device/CDev.cpp | 1 - pkgs/device/drv-device/CDev.hpp | 3 - pkgs/device/drv-device/Device.hpp | 3 +- pkgs/device/drv-device/nuttx/SPI.cpp | 2 +- pkgs/device/drv-device/posix/SPI.cpp | 2 - pkgs/device/drv-device/qurt/I2C.cpp | 3 +- pkgs/device/drv-device/qurt/SPI.cpp | 2 +- pkgs/device/drv-device/qurt/uart.c | 2 +- pkgs/load_mon/LoadMon.hpp | 19 +- pkgs/muorb/aggregator/mUORBAggregator.cpp | 200 +++--- pkgs/muorb/apps/uORBAppsProtobufChannel.hpp | 101 ++- pkgs/muorb/slpi/uORBProtobufChannel.cpp | 597 +++++++++--------- 197 files changed, 1566 insertions(+), 1656 deletions(-) diff --git a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index a764af1e7a..42ff7386f3 100644 --- a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp +++ b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -36,7 +36,6 @@ #include #include -// #include #include #include #include @@ -146,9 +145,7 @@ static uint8_t _auth_method_arm_req_check() { break; } - return state == ARM_AUTH_MISSION_APPROVED ? - vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : - vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; + return state == ARM_AUTH_MISSION_APPROVED ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; } static uint8_t _auth_method_two_arm_check() { diff --git a/apps/controller/commander/Commander.cpp b/apps/controller/commander/Commander.cpp index 1daa7cce7a..ba742cc770 100644 --- a/apps/controller/commander/Commander.cpp +++ b/apps/controller/commander/Commander.cpp @@ -30,19 +30,15 @@ /* headers */ #include +#include #include #include #include #include #include -#include -// #include -// #include -// #include -// #include -// #include +// #include +#include #include -#include #include #include #include @@ -62,13 +58,13 @@ typedef enum VEHICLE_MODE_FLAG { // TODO: generate static constexpr bool operator==(const actuator_armed_s &a, const actuator_armed_s &b) { - return (a.armed == b.armed && - a.prearmed == b.prearmed && - a.ready_to_arm == b.ready_to_arm && - a.lockdown == b.lockdown && - a.manual_lockdown == b.manual_lockdown && - a.force_failsafe == b.force_failsafe && - a.in_esc_calibration_mode == b.in_esc_calibration_mode); + return (a.armed == b.armed + && a.prearmed == b.prearmed + && a.ready_to_arm == b.ready_to_arm + && a.lockdown == b.lockdown + && a.manual_lockdown == b.manual_lockdown + && a.force_failsafe == b.force_failsafe + && a.in_esc_calibration_mode == b.in_esc_calibration_mode); } static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review"); @@ -515,8 +511,8 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks && !_arm_state_machine.isArmed()) { if (_vehicle_control_mode.flag_control_manual_enabled) { - if (_vehicle_control_mode.flag_control_climb_rate_enabled && - !_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) { + if (_vehicle_control_mode.flag_control_climb_rate_enabled + && !_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t"); events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info}, @@ -1773,8 +1769,8 @@ void Commander::checkForMissionUpdate() { if (mission_result_ok) { /* Only evaluate mission state if home is set */ - if (!_failsafe_flags.home_position_invalid && - (prev_mission_instance_count != mission_result.instance_count)) { + if (!_failsafe_flags.home_position_invalid + && (prev_mission_instance_count != mission_result.instance_count)) { if (!auto_mission_available) { /* the mission is invalid */ tune_mission_fail(true); @@ -1939,8 +1935,8 @@ void Commander::vtolStatusUpdate() { _status_changed = true; } - const bool new_in_transition = _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW || - _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; + const bool new_in_transition = _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW + || _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; if (_vehicle_status.in_transition_mode != new_in_transition) { _vehicle_status.in_transition_mode = new_in_transition; @@ -1961,14 +1957,14 @@ void Commander::updateTunes() { set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); _arm_tune_played = true; - } else if (!_vehicle_status.usb_connected && - (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + } else if (!_vehicle_status.usb_connected + && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) + && (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { /* play tune on battery critical */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); - } else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { + } else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) + && (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); diff --git a/apps/controller/commander/accelerometer_calibration.cpp b/apps/controller/commander/accelerometer_calibration.cpp index 63ef62d236..04028f4df3 100644 --- a/apps/controller/commander/accelerometer_calibration.cpp +++ b/apps/controller/commander/accelerometer_calibration.cpp @@ -103,8 +103,6 @@ #include "commander_helper.h" #include "factory_calibration_storage.h" #include -// #include -// #include #include #include #include diff --git a/apps/controller/commander/airspeed_calibration.cpp b/apps/controller/commander/airspeed_calibration.cpp index d49a8553f6..6d6f38d09c 100644 --- a/apps/controller/commander/airspeed_calibration.cpp +++ b/apps/controller/commander/airspeed_calibration.cpp @@ -18,8 +18,6 @@ #include "calibration_routines.h" #include "commander_helper.h" #include -// #include -// #include #include #include #include @@ -37,163 +35,163 @@ using namespace time_literals; static const char *sensor_name = "airspeed"; static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) { - rt_thread_mdelay(5000); - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + rt_thread_mdelay(5000); + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) { - const hrt_abstime calibration_started = hrt_absolute_time(); + const hrt_abstime calibration_started = hrt_absolute_time(); - int result = PX4_OK; - unsigned calibration_counter = 0; - const unsigned maxcount = 500; + int result = PX4_OK; + unsigned calibration_counter = 0; + const unsigned maxcount = 500; - /* give directions */ - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); + /* give directions */ + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = (maxcount * 2) / 3; + const unsigned calibration_count = (maxcount * 2) / 3; - float diff_pres_offset = 0.0f; + float diff_pres_offset = 0.0f; - calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); - usleep(500 * 1000); + calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); + usleep(500 * 1000); - uORB::SubscriptionData diff_pres_sub{ORB_ID(differential_pressure)}; + uORB::SubscriptionData diff_pres_sub{ORB_ID(differential_pressure)}; - while (calibration_counter < calibration_count) { - if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { - return PX4_ERROR; - } + while (calibration_counter < calibration_count) { + if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { + return PX4_ERROR; + } - if (diff_pres_sub.update()) { - const differential_pressure_s &diff_pres = diff_pres_sub.get(); + if (diff_pres_sub.update()) { + const differential_pressure_s &diff_pres = diff_pres_sub.get(); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) { - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); - } - } + if (calibration_counter % (calibration_count / 20) == 0) { + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + } + } - if (hrt_elapsed_time(&calibration_started) > 30_s) { - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; + if (hrt_elapsed_time(&calibration_started) > 30_s) { + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } + + usleep(10000); } - usleep(10000); - } + diff_pres_offset = diff_pres_offset / calibration_count; - diff_pres_offset = diff_pres_offset / calibration_count; + if (PX4_ISFINITE(diff_pres_offset)) { + // Prevent a completely zero param + // since this is used to detect a missing calibration + // This value is numerically down in the noise and has + // no effect on the sensor performance. + if (fabsf(diff_pres_offset) < 0.00000001f) { + diff_pres_offset = 0.00000001f; + } - if (PX4_ISFINITE(diff_pres_offset)) { - // Prevent a completely zero param - // since this is used to detect a missing calibration - // This value is numerically down in the noise and has - // no effect on the sensor performance. - if (fabsf(diff_pres_offset) < 0.00000001f) { - diff_pres_offset = 0.00000001f; - } + if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); + return PX4_ERROR; + } - if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - return PX4_ERROR; + } else { + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; } - } else { - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; - } + calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset); - calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); - /* wait 500 ms to ensure parameter propagated through the system */ - usleep(500 * 1000); + calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching"); - calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching"); + float differential_pressure_sum = 0.f; + int differential_pressure_sum_count = 0; - float differential_pressure_sum = 0.f; - int differential_pressure_sum_count = 0; + calibration_counter = 0; - calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < maxcount) { + if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { + return PX4_ERROR; + } - /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { - return PX4_ERROR; - } + if (diff_pres_sub.update()) { + const differential_pressure_s &diff_pres = diff_pres_sub.get(); - if (diff_pres_sub.update()) { - const differential_pressure_s &diff_pres = diff_pres_sub.get(); + differential_pressure_sum += diff_pres.differential_pressure_pa; + differential_pressure_sum_count++; - differential_pressure_sum += diff_pres.differential_pressure_pa; - differential_pressure_sum_count++; + const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset; - const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset; + if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) { + if (differential_pressure_pa > 0) { + calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa); + break; - if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) { - if (differential_pressure_pa > 0) { - calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa); - break; + } else { + /* do not allow negative values */ + calibration_log_critical(mavlink_log_pub, + "[cal] Negative pressure difference detected (%d Pa)", + (int)differential_pressure_pa); + calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!"); - } else { - /* do not allow negative values */ - calibration_log_critical(mavlink_log_pub, - "[cal] Negative pressure difference detected (%d Pa)", - (int)differential_pressure_pa); - calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!"); + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + diff_pres_offset = 0.0f; - /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ - diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); + return PX4_ERROR; + } - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - return PX4_ERROR; - } + /* save */ + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); + param_save_default(); - /* save */ - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - param_save_default(); + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } + } - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; - } - } + if (calibration_counter % 300 == 0) { + calibration_log_info(mavlink_log_pub, + "[cal] Create air pressure! (got %d, wanted: 50 Pa)", + (int)differential_pressure_pa); + tune_neutral(true); - if (calibration_counter % 300 == 0) { - calibration_log_info(mavlink_log_pub, - "[cal] Create air pressure! (got %d, wanted: 50 Pa)", - (int)differential_pressure_pa); - tune_neutral(true); + // reset average + differential_pressure_sum = 0.f; + differential_pressure_sum_count = 0; + } - // reset average - differential_pressure_sum = 0.f; - differential_pressure_sum_count = 0; - } + calibration_counter++; + } - calibration_counter++; - } + if (hrt_elapsed_time(&calibration_started) > 90_s) { + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } - if (hrt_elapsed_time(&calibration_started) > 90_s) { - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; + usleep(10000); } - usleep(10000); - } - - if (calibration_counter == maxcount) { - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; - } + if (calibration_counter == maxcount) { + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); - tune_neutral(true); + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + tune_neutral(true); - // This give a chance for the log messages to go out of the queue before someone else stomps on then - usleep(200000); + // This give a chance for the log messages to go out of the queue before someone else stomps on then + usleep(200000); - return result; + return result; } diff --git a/apps/controller/commander/baro_calibration.cpp b/apps/controller/commander/baro_calibration.cpp index ec8fb68b66..5201dc1028 100644 --- a/apps/controller/commander/baro_calibration.cpp +++ b/apps/controller/commander/baro_calibration.cpp @@ -19,8 +19,7 @@ #include "calibration_routines.h" #include "calibration_messages.h" #include -// #include -// #include + #include #include #include diff --git a/apps/controller/commander/calibration_routines.cpp b/apps/controller/commander/calibration_routines.cpp index dc53ace88e..ab5b43f250 100644 --- a/apps/controller/commander/calibration_routines.cpp +++ b/apps/controller/commander/calibration_routines.cpp @@ -17,8 +17,7 @@ #include #include -// #include -// #include + #include #include #include diff --git a/apps/controller/commander/commander_helper.cpp b/apps/controller/commander/commander_helper.cpp index 7332de9f27..459caab02a 100644 --- a/apps/controller/commander/commander_helper.cpp +++ b/apps/controller/commander/commander_helper.cpp @@ -19,7 +19,6 @@ */ #include -// #include #include #include #include diff --git a/apps/controller/commander/esc_calibration.cpp b/apps/controller/commander/esc_calibration.cpp index 65493a415a..a2e8f6891e 100644 --- a/apps/controller/commander/esc_calibration.cpp +++ b/apps/controller/commander/esc_calibration.cpp @@ -22,8 +22,6 @@ #include #include #include -// #include -// #include #include #include #include diff --git a/apps/controller/commander/failure_detector/failure_detector_params.c b/apps/controller/commander/failure_detector/failure_detector_params.c index 6559b23fa2..902ba08715 100644 --- a/apps/controller/commander/failure_detector/failure_detector_params.c +++ b/apps/controller/commander/failure_detector/failure_detector_params.c @@ -16,7 +16,6 @@ * @author Mathieu Bresciani */ -// #include #include /** diff --git a/apps/controller/commander/gyro_calibration.cpp b/apps/controller/commander/gyro_calibration.cpp index 60d57c7016..078ff3c9e2 100644 --- a/apps/controller/commander/gyro_calibration.cpp +++ b/apps/controller/commander/gyro_calibration.cpp @@ -14,16 +14,13 @@ * Gyroscope calibration routine */ -// #include #include "factory_calibration_storage.h" #include "gyro_calibration.h" #include "calibration_messages.h" #include "calibration_routines.h" #include "commander_helper.h" -// #include #include -// #include #include #include #include @@ -32,7 +29,6 @@ #include #include #include -#include #include static constexpr char sensor_name[]{"gyro"}; diff --git a/apps/controller/commander/level_calibration.cpp b/apps/controller/commander/level_calibration.cpp index 2d4875cc6c..38647b93c5 100644 --- a/apps/controller/commander/level_calibration.cpp +++ b/apps/controller/commander/level_calibration.cpp @@ -13,8 +13,7 @@ #include "calibration_routines.h" #include "commander_helper.h" #include -// #include -// #include + #include #include #include diff --git a/apps/controller/commander/mag_calibration.cpp b/apps/controller/commander/mag_calibration.cpp index 7610517423..ae50900ff3 100644 --- a/apps/controller/commander/mag_calibration.cpp +++ b/apps/controller/commander/mag_calibration.cpp @@ -21,8 +21,6 @@ #include "calibration_messages.h" #include "factory_calibration_storage.h" #include -// #include -// #include #include #include #include diff --git a/apps/controller/commander/rc_calibration.cpp b/apps/controller/commander/rc_calibration.cpp index 349a9a7ecd..597cce44d6 100644 --- a/apps/controller/commander/rc_calibration.cpp +++ b/apps/controller/commander/rc_calibration.cpp @@ -14,8 +14,6 @@ */ #include -// #include -// #include #include #include "rc_calibration.h" #include "commander_helper.h" diff --git a/apps/controller/commander/worker_thread.cpp b/apps/controller/commander/worker_thread.cpp index 3536e405c7..ae4fa86033 100644 --- a/apps/controller/commander/worker_thread.cpp +++ b/apps/controller/commander/worker_thread.cpp @@ -21,7 +21,7 @@ #include #include #include -// #include +#include #include using namespace time_literals; diff --git a/apps/controller/commander/worker_thread.hpp b/apps/controller/commander/worker_thread.hpp index 5f176b25d0..776e7684e7 100644 --- a/apps/controller/commander/worker_thread.hpp +++ b/apps/controller/commander/worker_thread.hpp @@ -11,7 +11,6 @@ #pragma once #include -// #include #include #include #include diff --git a/apps/controller/fw_att_control/FixedwingAttitudeControl.hpp b/apps/controller/fw_att_control/FixedwingAttitudeControl.hpp index 2fbf24b633..f36d891a04 100644 --- a/apps/controller/fw_att_control/FixedwingAttitudeControl.hpp +++ b/apps/controller/fw_att_control/FixedwingAttitudeControl.hpp @@ -20,13 +20,12 @@ // #include // #include // #include -// #include // #include // #include // #include -// #include -// #include + + // #include // #include // #include @@ -75,7 +74,7 @@ class FixedwingAttitudeControl final : public ModuleCommand // #include // #include -// #include -// #include +#include // #include // #include -// #include // #include // #include // #include diff --git a/apps/controller/fw_pos_control/FixedwingPositionControl.hpp b/apps/controller/fw_pos_control/FixedwingPositionControl.hpp index a127e7a947..7b350c0383 100644 --- a/apps/controller/fw_pos_control/FixedwingPositionControl.hpp +++ b/apps/controller/fw_pos_control/FixedwingPositionControl.hpp @@ -35,17 +35,14 @@ // #include // #include #include -// #include // #include // #include // #include -// #include -// #include -// #include + +#include // #include // #include -// #include // #include // #include // #include @@ -260,8 +257,8 @@ class FixedwingPositionControl final : public ModuleCommand #include // #include -// #include // #include // #include // #include -// #include -// #include // #include // #include // #include diff --git a/apps/controller/land_detector/LandDetector.h b/apps/controller/land_detector/LandDetector.h index d72e2a0367..d1171dfe89 100644 --- a/apps/controller/land_detector/LandDetector.h +++ b/apps/controller/land_detector/LandDetector.h @@ -28,7 +28,6 @@ // #include // #include // #include -// #include // #include // #include @@ -120,21 +119,27 @@ class LandDetector : public ModuleCommand, ModuleParams, WorkItemS virtual bool _get_in_descend() { return false; } + virtual bool _get_has_low_throttle() { return false; } + virtual bool _get_horizontal_movement() { return false; } + virtual bool _get_vertical_movement() { return false; } + virtual bool _get_rotational_movement() { return false; } + virtual bool _get_close_to_ground_or_skipped_check() { return false; } + virtual void _set_hysteresis_factor(const int factor) = 0; Hysteresis _freefall_hysteresis{false}; @@ -162,7 +167,7 @@ class LandDetector : public ModuleCommand, ModuleParams, WorkItemS hrt_abstime _takeoff_time{0}; hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds - hrt_abstime _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds + hrt_abstime _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")}; diff --git a/apps/controller/land_detector/land_detector_main.cpp b/apps/controller/land_detector/land_detector_main.cpp index b83cc71e0c..dfb2ebf133 100644 --- a/apps/controller/land_detector/land_detector_main.cpp +++ b/apps/controller/land_detector/land_detector_main.cpp @@ -19,11 +19,9 @@ #define LOG_TAG "land_detector_main" // #include -// #include // #include -// #include -// #include + #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" @@ -85,6 +83,7 @@ int LandDetector::print_status() { PX4_INFO("running (%s)", _currentMode); return 0; } + int LandDetector::print_usage(const char *reason) { if (reason != nullptr) { PX4_ERR("%s\n", reason); @@ -126,6 +125,7 @@ The module runs periodically on the HP work queue. extern "C" __EXPORT int land_detector_main(int argc, char *argv[]) { return LandDetector::main(argc, argv); } + MSH_CMD_EXPORT_ALIAS(land_detector_main, land_detector, land detector); int land_detector_start() { @@ -139,6 +139,7 @@ int land_detector_start() { int argc = sizeof(argv) / sizeof(argv[0]); return LandDetector::main(argc, (char **)argv); } + INIT_APP_EXPORT(land_detector_start); } // namespace land_detector diff --git a/apps/controller/landing_target_estimator/LandingTargetEstimator.cpp b/apps/controller/landing_target_estimator/LandingTargetEstimator.cpp index 7b914a88c5..11be212f41 100644 --- a/apps/controller/landing_target_estimator/LandingTargetEstimator.cpp +++ b/apps/controller/landing_target_estimator/LandingTargetEstimator.cpp @@ -16,7 +16,6 @@ * */ -// #include #include #include diff --git a/apps/controller/landing_target_estimator/LandingTargetEstimator.h b/apps/controller/landing_target_estimator/LandingTargetEstimator.h index ef39c5f554..b56cd7a4dd 100644 --- a/apps/controller/landing_target_estimator/LandingTargetEstimator.h +++ b/apps/controller/landing_target_estimator/LandingTargetEstimator.h @@ -19,12 +19,11 @@ #pragma once -#include +#include #include #include #include #include -#include #include #include #include diff --git a/apps/controller/landing_target_estimator/landing_target_estimator_main.cpp b/apps/controller/landing_target_estimator/landing_target_estimator_main.cpp index c13ee15a8e..0ae745a090 100644 --- a/apps/controller/landing_target_estimator/landing_target_estimator_main.cpp +++ b/apps/controller/landing_target_estimator/landing_target_estimator_main.cpp @@ -17,10 +17,9 @@ * */ -// #include #include -#include -#include + + #include #include #include diff --git a/apps/controller/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/apps/controller/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp index f5b82fbeff..f80e732ee3 100644 --- a/apps/controller/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp +++ b/apps/controller/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp @@ -23,8 +23,8 @@ // #include // #include // #include -// #include -// #include + +#include // #include // #include // #include @@ -144,7 +144,7 @@ class McAutotuneAttitudeControl : public ModuleCommand _signal_filter; ///< used to create a wash-out filter + AlphaFilter _signal_filter; ///< used to create a wash-out filter static constexpr float _model_dt_min{2e-3f}; // 2ms = 500Hz static constexpr float _model_dt_max{10e-3f}; // 10ms = 100Hz diff --git a/apps/controller/mc_pos_control/MulticopterPositionControl.hpp b/apps/controller/mc_pos_control/MulticopterPositionControl.hpp index 83684a6fc2..0e00490982 100644 --- a/apps/controller/mc_pos_control/MulticopterPositionControl.hpp +++ b/apps/controller/mc_pos_control/MulticopterPositionControl.hpp @@ -23,17 +23,15 @@ #include // #include -// #include // #include // #include // #include // #include -// #include -// #include + + // #include // #include -// #include // #include // #include // #include @@ -73,7 +71,7 @@ class MulticopterPositionControl : public ModuleCommand _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -158,9 +156,9 @@ class MulticopterPositionControl : public ModuleCommand // #include // #include // #include -// #include -// #include + +#include // #include // #include // #include diff --git a/apps/controller/vtol_att_control/vtol_att_control_main.h b/apps/controller/vtol_att_control/vtol_att_control_main.h index 0bd77dd15f..8db59ef2a5 100644 --- a/apps/controller/vtol_att_control/vtol_att_control_main.h +++ b/apps/controller/vtol_att_control/vtol_att_control_main.h @@ -31,17 +31,11 @@ // #include // #include // #include -// #include - // #include // #include // #include -// #include -// #include -// #include // #include // #include -// #include // #include // #include // #include @@ -91,7 +85,7 @@ class VtolAttitudeControl : public ModuleCommand, public Mo bool is_fixed_wing_requested() { return _transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - }; + } void quadchute(QuadchuteReason reason); @@ -114,12 +108,15 @@ class VtolAttitudeControl : public ModuleCommand, public Mo struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_mc() { return &_vehicle_torque_setpoint_virtual_mc; } + struct vehicle_torque_setpoint_s *get_vehicle_torque_setpoint_virtual_fw() { return &_vehicle_torque_setpoint_virtual_fw; } + struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_mc() { return &_vehicle_thrust_setpoint_virtual_mc; } + struct vehicle_thrust_setpoint_s *get_vehicle_thrust_setpoint_virtual_fw() { return &_vehicle_thrust_setpoint_virtual_fw; } @@ -127,51 +124,67 @@ class VtolAttitudeControl : public ModuleCommand, public Mo struct airspeed_validated_s *get_airspeed() { return &_airspeed_validated; } + struct position_setpoint_triplet_s *get_pos_sp_triplet() { return &_pos_sp_triplet; } + struct tecs_status_s *get_tecs_status() { return &_tecs_status; } + struct vehicle_attitude_s *get_att() { return &_vehicle_attitude; } + struct vehicle_attitude_setpoint_s *get_att_sp() { return &_vehicle_attitude_sp; } + struct vehicle_attitude_setpoint_s *get_fw_virtual_att_sp() { return &_fw_virtual_att_sp; } + struct vehicle_attitude_setpoint_s *get_mc_virtual_att_sp() { return &_mc_virtual_att_sp; } + struct vehicle_control_mode_s *get_control_mode() { return &_vehicle_control_mode; } + struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } + struct vehicle_local_position_s *get_local_pos() { return &_local_pos; } + struct vehicle_local_position_setpoint_s *get_local_pos_sp() { return &_local_pos_sp; } + struct vehicle_torque_setpoint_s *get_torque_setpoint_0() { return &_torque_setpoint_0; } + struct vehicle_torque_setpoint_s *get_torque_setpoint_1() { return &_torque_setpoint_1; } + struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() { return &_thrust_setpoint_0; } + struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() { return &_thrust_setpoint_1; } + struct vtol_vehicle_status_s *get_vtol_vehicle_status() { return &_vtol_vehicle_status; } + float get_home_position_z() { return _home_position_z; } @@ -210,7 +223,7 @@ class VtolAttitudeControl : public ModuleCommand, public Mo uORB::PublicationMulti _vehicle_torque_setpoint1_pub{ORB_ID(vehicle_torque_setpoint)}; uORB::Publication _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; - orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle + orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle vehicle_attitude_setpoint_s _vehicle_attitude_sp{}; // vehicle attitude setpoint vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint diff --git a/apps/estimator/attitude_estimator_q/attitude_estimator_q_main.cpp b/apps/estimator/attitude_estimator_q/attitude_estimator_q_main.cpp index a40941e971..d2fc0e1073 100644 --- a/apps/estimator/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/apps/estimator/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -8,6 +8,8 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ +#define LOG_TAG "att_est_q" + /* * @file attitude_estimator_q_main.cpp * @@ -27,10 +29,9 @@ #include #include #include -// #include + #include #include -#include #include #include #include @@ -46,6 +47,8 @@ using matrix::Vector3f; using matrix::wrap_pi; using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; class AttitudeEstimatorQ : public ModuleCommand, public ModuleParams, public WorkItem { public: @@ -53,7 +56,7 @@ class AttitudeEstimatorQ : public ModuleCommand, public Modu ~AttitudeEstimatorQ() override = default; /** @see ModuleCommand */ - static int *instantiate(int argc, char *argv[]); + static AttitudeEstimatorQ *instantiate(int argc, char *argv[]); /** @see ModuleCommand */ static int custom_command(int argc, char *argv[]); @@ -150,13 +153,13 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : update_parameters(true); } -bool AttitudeEstimatorQ::init() { +int AttitudeEstimatorQ::init() { if (!_sensors_sub.registerCallback()) { PX4_ERR("callback registration failed"); - return false; + return RT_ERROR; } - return true; + return RT_EOK; } void AttitudeEstimatorQ::Run() { @@ -218,11 +221,7 @@ void AttitudeEstimatorQ::update_motion_capture_odometry() { if (_vehicle_mocap_odometry_sub.update(&mocap)) { // validation check for mocap attitude data - bool mocap_att_valid = PX4_ISFINITE(mocap.q[0]) && (PX4_ISFINITE(mocap.orientation_variance[0]) ? sqrtf(fmaxf( - mocap.orientation_variance[0], - fmaxf(mocap.orientation_variance[1], - mocap.orientation_variance[2]))) <= _eo_max_std_dev : - true); + bool mocap_att_valid = PX4_ISFINITE(mocap.q[0]) && (PX4_ISFINITE(mocap.orientation_variance[0]) ? sqrtf(fmaxf(mocap.orientation_variance[0], fmaxf(mocap.orientation_variance[1], mocap.orientation_variance[2]))) <= _eo_max_std_dev : true); if (mocap_att_valid) { Dcmf Rmoc = Quatf(mocap.q); @@ -320,11 +319,7 @@ void AttitudeEstimatorQ::update_visual_odometry() { if (_vehicle_visual_odometry_sub.update(&vision)) { // validation check for vision attitude data - bool vision_att_valid = PX4_ISFINITE(vision.q[0]) && (PX4_ISFINITE(vision.orientation_variance[0]) ? sqrtf(fmaxf( - vision.orientation_variance[0], - fmaxf(vision.orientation_variance[1], - vision.orientation_variance[2]))) <= _eo_max_std_dev : - true); + bool vision_att_valid = PX4_ISFINITE(vision.q[0]) && (PX4_ISFINITE(vision.orientation_variance[0]) ? sqrtf(fmaxf(vision.orientation_variance[0], fmaxf(vision.orientation_variance[1], vision.orientation_variance[2]))) <= _eo_max_std_dev : true); if (vision_att_valid) { Dcmf Rvis = Quatf(vision.q); @@ -477,8 +472,7 @@ bool AttitudeEstimatorQ::update(float dt) { const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; - if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) && - (accel_norm_sq < upper_accel_limit * upper_accel_limit))) { + if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) && (accel_norm_sq < upper_accel_limit * upper_accel_limit))) { corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get(); } @@ -530,26 +524,27 @@ int AttitudeEstimatorQ::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int AttitudeEstimatorQ::instantiate(int argc, char *argv[]) { +AttitudeEstimatorQ *AttitudeEstimatorQ::instantiate(int argc, char *argv[]) { AttitudeEstimatorQ *instance = new AttitudeEstimatorQ(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + // _object.store(instance); + // _task_id = task_id_is_work_queue; - if (instance->init()) { - return PX4_OK; - } + // if (instance->init()) { + // return PX4_OK; + // } + return instance; } else { PX4_ERR("alloc failed"); } delete instance; - _object.store(nullptr); - _task_id = -1; + // _object.store(nullptr); + // _task_id = -1; - return PX4_ERROR; + return nullptr; } int AttitudeEstimatorQ::print_usage(const char *reason) { diff --git a/apps/estimator/ekf2/EKF2.hpp b/apps/estimator/ekf2/EKF2.hpp index fad2f6def6..282ca00b7a 100644 --- a/apps/estimator/ekf2/EKF2.hpp +++ b/apps/estimator/ekf2/EKF2.hpp @@ -33,9 +33,9 @@ #include #include #include -#include + #include -#include + #include #include #include @@ -69,21 +69,21 @@ #include #if defined(CONFIG_EKF2_AIRSPEED) -#include -#include +# include +# include #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_AUXVEL) -#include +# include #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) -#include -#include +# include +# include #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_RANGE_FINDER) -#include +# include #endif // CONFIG_EKF2_RANGE_FINDER extern pthread_mutex_t ekf2_module_mutex; @@ -116,9 +116,11 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); } + static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); } + static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); } @@ -208,8 +210,8 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { // Used to check, save and use learned accel/gyro/mag biases struct InFlightCalibration { - hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec) - hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save + hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec) + hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save matrix::Vector3f bias{}; bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available }; @@ -242,7 +244,7 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { static constexpr float sq(float x) { return x * x; - }; + } const bool _replay_mode{false}; ///< true when we use replay data from a log const bool _multi_mode; @@ -460,11 +462,11 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { (ParamExtInt)_param_ekf2_imu_ctrl, (ParamExtFloat) - _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) + _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) - _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) + _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) @@ -486,7 +488,7 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { (ParamExtFloat) _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) (ParamExtFloat) - _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) + _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) (ParamExtFloat) _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) @@ -495,23 +497,23 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { (ParamExtFloat) _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) (ParamExtFloat) - _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) + _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) (ParamExtFloat) - _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) + _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) + _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) (ParamExtFloat) _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) (ParamExtFloat) - _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) + _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) + _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) #if defined(CONFIG_EKF2_AIRSPEED) (ParamExtFloat) _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) (ParamExtFloat) - _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) + _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) @@ -531,32 +533,32 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { // control of magnetometer fusion (ParamExtFloat) - _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) + _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) (ParamExtFloat) - _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) (ParamExtFloat)_param_ekf2_mag_decl, ///< magnetic declination (degrees) (ParamExtFloat) - _param_ekf2_hdg_gate, ///< heading fusion innovation consistency gate size (STD) + _param_ekf2_hdg_gate, ///< heading fusion innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) + _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) (ParamExtInt) - _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data + _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data (ParamExtInt) - _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used + _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used (ParamExtFloat) - _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used + _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used (ParamExtFloat) - _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) + _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) (ParamExtInt) - _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used - (ParamExtFloat)_param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) - (ParamExtFloat)_param_ekf2_req_epv, ///< maximum acceptable vert position error (m) - (ParamExtFloat)_param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) - (ParamExtInt)_param_ekf2_req_nsats, ///< minimum acceptable satellite count + _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used + (ParamExtFloat)_param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) + (ParamExtFloat)_param_ekf2_req_epv, ///< maximum acceptable vert position error (m) + (ParamExtFloat)_param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) + (ParamExtInt)_param_ekf2_req_nsats, ///< minimum acceptable satellite count (ParamExtFloat) - _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision + _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision (ParamExtFloat) _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) (ParamExtFloat)_param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) @@ -573,46 +575,46 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion - (ParamExtInt)_param_ekf2_rng_ctrl, ///< range finder control selection + (ParamExtInt)_param_ekf2_rng_ctrl, ///< range finder control selection (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) + _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) + _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) (ParamExtFloat) - _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) + _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) + _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) (ParamExtFloat)_param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) + _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) + _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) + _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) (ParamExtFloat) - _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) (ParamExtFloat) - _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) + _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) (ParamExtFloat) - _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) + _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) (ParamExtFloat) - _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) + _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) (ParamExtFloat) - _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) + _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) (ParamExtInt) _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation (ParamExtFloat)_param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) -#endif // CONFIG_EKF2_RANGE_FINDER + _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision estimate fusion (ParamExtFloat) - _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) + _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) (ParamExtInt)_param_ekf2_ev_ctrl, ///< external vision (EV) control selection (ParamInt)_param_ekf2_ev_noise_md, ///< determine source of vision observation noise @@ -624,9 +626,9 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { (ParamExtFloat) _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) (ParamExtFloat) - _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) + _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) (ParamExtFloat) _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) @@ -642,26 +644,26 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion (ParamExtInt) - _param_ekf2_of_ctrl, ///< optical flow fusion selection + _param_ekf2_of_ctrl, ///< optical flow fusion selection (ParamExtFloat) - _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval + _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval (ParamExtFloat) - _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) + _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtFloat) - _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) + _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air (ParamExtInt) _param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground (ParamExtFloat) - _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) (ParamExtFloat) - _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) + _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) (ParamExtFloat) - _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) + _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) (ParamExtFloat) - _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) -#endif // CONFIG_EKF2_OPTICAL_FLOW + _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) +#endif // CONFIG_EKF2_OPTICAL_FLOW // sensor positions in body frame (ParamExtFloat)_param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) @@ -679,20 +681,20 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { // IMU switch on bias parameters (ParamExtFloat) - _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) (ParamExtFloat) - _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) + _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) (ParamExtFloat) _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) // EKF accel bias learning control - (ParamExtFloat)_param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) + (ParamExtFloat)_param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) (ParamExtFloat) - _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) + _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) (ParamExtFloat) - _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) + _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) (ParamExtFloat) - _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) + _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) (ParamExtFloat)_param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) @@ -710,23 +712,23 @@ class EKF2 final : public ModuleParams, public WorkItemScheduled { // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 (ParamExtFloat) - _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) + _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) (ParamExtFloat) - _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis + _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis (ParamExtFloat) - _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis + _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis (ParamExtFloat) - _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis + _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis (ParamExtFloat) - _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis + _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis (ParamExtFloat) - _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis -#endif // CONFIG_EKF2_BARO_COMPENSATION + _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis +#endif // CONFIG_EKF2_BARO_COMPENSATION (ParamFloat)_param_ekf2_req_gps_h, ///< Required GPS health time (ParamExtInt)_param_ekf2_mag_check, ///< Mag field strength check (ParamExtInt) - _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. + _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) diff --git a/apps/estimator/ekf2/EKF2Selector.hpp b/apps/estimator/ekf2/EKF2Selector.hpp index a322e4dd08..1f396edb40 100644 --- a/apps/estimator/ekf2/EKF2Selector.hpp +++ b/apps/estimator/ekf2/EKF2Selector.hpp @@ -11,12 +11,11 @@ #ifndef EKF2SELECTOR_HPP #define EKF2SELECTOR_HPP -// #include #include #include #include -#include + #include #include #include @@ -35,9 +34,9 @@ #include #if CONSTRAINED_MEMORY -#define EKF2_MAX_INSTANCES 2 +# define EKF2_MAX_INSTANCES 2 #else -#define EKF2_MAX_INSTANCES 9 +# define EKF2_MAX_INSTANCES 9 #endif using namespace time_literals; @@ -123,32 +122,28 @@ class EKF2Selector : public ModuleParams, public WorkItemScheduled { static constexpr float _rel_err_score_lim{1.0f}; // +- limit applied to the relative error score static constexpr float _rel_err_thresh{0.5f}; // the relative score difference needs to be greater than this to switch from an otherwise healthy instance - EstimatorInstance _instance[EKF2_MAX_INSTANCES]{ + EstimatorInstance _instance[EKF2_MAX_INSTANCES] { {this, 0}, - {this, 1}, + {this, 1}, #if EKF2_MAX_INSTANCES > 2 - {this, 2}, - {this, 3}, -#if EKF2_MAX_INSTANCES > 4 - {this, 4}, - {this, 5}, - {this, 6}, - {this, 7}, - {this, 8}, -#endif + {this, 2}, + {this, 3}, +# if EKF2_MAX_INSTANCES > 4 + {this, 4}, + {this, 5}, + {this, 6}, + {this, 7}, + {this, 8}, +# endif #endif - }; + } - static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( - sensors_status_imu_s::gyro_inconsistency_rad_s[0])); - static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof( - estimator_selector_status_s::accumulated_gyro_error[0]), + static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s[0])); + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof(estimator_selector_status_s::accumulated_gyro_error[0]), "increase estimator_selector_status_s::accumulated_gyro_error size"); - static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof( - estimator_selector_status_s::accumulated_accel_error[0]), + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof(estimator_selector_status_s::accumulated_accel_error[0]), "increase estimator_selector_status_s::accumulated_accel_error size"); - static_assert(EKF2_MAX_INSTANCES <= sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof( - estimator_selector_status_s::combined_test_ratio[0]), + static_assert(EKF2_MAX_INSTANCES <= sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof(estimator_selector_status_s::combined_test_ratio[0]), "increase estimator_selector_status_s::combined_test_ratio size"); float _accumulated_gyro_error[IMU_STATUS_SIZE]{}; diff --git a/apps/estimator/gyro_calibration/GyroCalibration.cpp b/apps/estimator/gyro_calibration/GyroCalibration.cpp index dbffaa303b..0c8a51e6a5 100644 --- a/apps/estimator/gyro_calibration/GyroCalibration.cpp +++ b/apps/estimator/gyro_calibration/GyroCalibration.cpp @@ -8,6 +8,8 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ +#define LOG_TAG "gyro_cali" + #include "GyroCalibration.hpp" #include @@ -25,9 +27,9 @@ GyroCalibration::~GyroCalibration() { perf_free(_calibration_updated_perf); } -bool GyroCalibration::init() { +int GyroCalibration::init() { ScheduleOnInterval(INTERVAL_US); - return true; + return 0; } void GyroCalibration::Run() { @@ -241,26 +243,26 @@ void GyroCalibration::Run() { } } -int GyroCalibration::instantiate(int argc, char *argv[]) { +GyroCalibration *GyroCalibration::instantiate(int argc, char *argv[]) { GyroCalibration *instance = new GyroCalibration(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } + // _object.store(instance); + // _task_id = task_id_is_work_queue; + // if (instance->init()) { + // return PX4_OK; + // } + return instance; } else { PX4_ERR("alloc failed"); } delete instance; - _object.store(nullptr); - _task_id = -1; + // _object.store(nullptr); + // _task_id = -1; - return PX4_ERROR; + return nullptr; } int GyroCalibration::print_status() { diff --git a/apps/estimator/gyro_calibration/GyroCalibration.hpp b/apps/estimator/gyro_calibration/GyroCalibration.hpp index 1e72eca543..c43c09aab8 100644 --- a/apps/estimator/gyro_calibration/GyroCalibration.hpp +++ b/apps/estimator/gyro_calibration/GyroCalibration.hpp @@ -13,21 +13,21 @@ #include #include #include -#include + #include #include #include #include #include #include -#include -#include #include #include #include #include using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; class GyroCalibration : public ModuleCommand, public ModuleParams, public WorkItemScheduled { public: @@ -35,7 +35,7 @@ class GyroCalibration : public ModuleCommand, public ModulePara ~GyroCalibration() override; /** @see ModuleCommand */ - static int *instantiate(int argc, char *argv[]); + static GyroCalibration *instantiate(int argc, char *argv[]); /** @see ModuleCommand */ static int custom_command(int argc, char *argv[]); diff --git a/apps/estimator/gyro_fft/GyroFFT.hpp b/apps/estimator/gyro_fft/GyroFFT.hpp index 38541bd938..52f07a0344 100644 --- a/apps/estimator/gyro_fft/GyroFFT.hpp +++ b/apps/estimator/gyro_fft/GyroFFT.hpp @@ -17,11 +17,9 @@ #include #include #include -#include + #include #include -#include -#include #include #include #include @@ -57,8 +55,7 @@ class GyroFFT : public ModuleCommand, public ModuleParams, public WorkI private: static constexpr int MAX_SENSOR_COUNT = 4; - static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( - sensor_gyro_fft_s::peak_frequencies_x[0]); + static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(sensor_gyro_fft_s::peak_frequencies_x[0]); void Run() override; inline void FindPeaks(const hrt_abstime ×tamp_sample, int axis, q15_t *fft_outupt_buffer); diff --git a/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.cpp b/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.cpp index 7e750e79cd..7c773013be 100644 --- a/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -1,7 +1,7 @@ #include "BlockLocalPositionEstimator.hpp" #include #include -#include +// #include #include #include @@ -12,10 +12,10 @@ static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m -static const float P_MAX = 1.0e6f; // max allowed value in state covariance -static const float LAND_RATE = 10.0f; // rate of land detector correction +static const float P_MAX = 1.0e6f; // max allowed value in state covariance +static const float LAND_RATE = 10.0f; // rate of land detector correction -static const char *msg_label = "[lpe] "; // rate of land detector correction +static const char *msg_label = "[lpe] "; // rate of land detector correction BlockLocalPositionEstimator::BlockLocalPositionEstimator() : ModuleParams(nullptr), @@ -133,13 +133,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : (_param_lpe_fusion.get() & FUSE_BARO) != 0); } -bool BlockLocalPositionEstimator::init() { +int BlockLocalPositionEstimator::init() { if (!_sensors_sub.registerCallback()) { PX4_ERR("callback registration failed"); - return false; + return -1; } - return true; + return 0; } Vector BlockLocalPositionEstimator::dynamics( @@ -195,20 +195,20 @@ void BlockLocalPositionEstimator::Run() { for (size_t i = 0; i < N_DIST_SUBS; i++) { auto *s = _dist_subs[i]; - if (s == _sub_lidar || s == _sub_sonar) { continue; } + if (s == _sub_lidar || s == _sub_sonar) { + continue; + } if (s->update()) { - if (s->get().timestamp == 0) { continue; } + if (s->get().timestamp == 0) { + continue; + } - if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && - s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && - _sub_lidar == nullptr) { + if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_lidar == nullptr) { _sub_lidar = s; mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i); - } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && - s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && - _sub_sonar == nullptr) { + } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_sonar == nullptr) { _sub_sonar = s; mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i); } @@ -392,10 +392,14 @@ void BlockLocalPositionEstimator::Run() { m_P(j, i) = m_P(i, j); } - if (reinit_P) { break; } + if (reinit_P) { + break; + } } - if (reinit_P) { break; } + if (reinit_P) { + break; + } } if (reinit_P) { @@ -511,8 +515,7 @@ void BlockLocalPositionEstimator::Run() { // needs to be propagated with frozen state float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist); - if (_time_last_hist == 0 || - (dt_hist > HIST_STEP)) { + if (_time_last_hist == 0 || (dt_hist > HIST_STEP)) { _tDelay.update(Scalar(_timeStamp)); _xDelay.update(_x); _time_last_hist = _timeStamp; @@ -566,8 +569,7 @@ void BlockLocalPositionEstimator::publishLocalPos() { } // publish local position - if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() && - Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) { + if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() && Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) { _pub_lpos.get().timestamp_sample = _timeStamp; _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY; @@ -628,8 +630,7 @@ void BlockLocalPositionEstimator::publishOdom() { const Vector &xLP = _xLowPass.getState(); // publish vehicle odometry - if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() && - Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) { + if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() && Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) { _pub_odom.get().timestamp_sample = _timeStamp; _pub_odom.get().pose_frame = vehicle_odometry_s::POSE_FRAME_NED; @@ -774,8 +775,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() { } } - if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && - Vector3f(xLP(X_vx), xLP(X_vy), xLP(X_vz)).isAllFinite()) { + if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && Vector3f(xLP(X_vx), xLP(X_vy), xLP(X_vz)).isAllFinite()) { _pub_gpos.get().timestamp_sample = _timeStamp; _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; @@ -872,8 +872,7 @@ void BlockLocalPositionEstimator::updateSSParams() { // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity float pn_t_noise_density = - _param_lpe_pn_t.get() + - (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy)); + _param_lpe_pn_t.get() + (_param_lpe_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy)); m_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density; } @@ -882,7 +881,7 @@ void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu) { _R_att = matrix::Dcm(matrix::Quatf(_sub_att.get().q)); Vector3f a(imu.accelerometer_m_s2); // note, bias is removed in dynamics function - _u = _R_att * a; + _u = _R_att * a; _u(U_az) += CONSTANTS_ONE_G; // add g // update state space based on new states @@ -939,10 +938,8 @@ void BlockLocalPositionEstimator::predict(const sensor_combined_s &imu) { } // propagate - _x += dx; - Matrix dP = (m_A * m_P + m_P * m_A.transpose() + - m_B * m_R * m_B.transpose() + m_Q) * - getDt(); + _x += dx; + Matrix dP = (m_A * m_P + m_P * m_A.transpose() + m_B * m_R * m_B.transpose() + m_Q) * getDt(); // covariance propagation logic for (size_t i = 0; i < n_x; i++) { @@ -988,28 +985,26 @@ int BlockLocalPositionEstimator::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int - BlockLocalPositionEstimator::* - instantiate(int argc, char *argv[]) { +BlockLocalPositionEstimator *BlockLocalPositionEstimator::instantiate(int argc, char *argv[]) { BlockLocalPositionEstimator *instance = new BlockLocalPositionEstimator(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } + // _object.store(instance); + // _task_id = task_id_is_work_queue; + // if (instance->init()) { + // return PX4_OK; + // } + return instance; } else { PX4_ERR("alloc failed"); } delete instance; - _object.store(nullptr); - _task_id = -1; + // _object.store(nullptr); + // _task_id = -1; - return PX4_ERROR; + return nullptr; } int BlockLocalPositionEstimator::print_usage(const char *reason) { diff --git a/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.hpp b/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.hpp index f75cb205be..f6de25feab 100644 --- a/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/apps/estimator/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -11,7 +10,6 @@ // uORB Subscriptions #include -#include #include #include #include @@ -40,11 +38,13 @@ using namespace matrix; using namespace control; using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; static const float DELAY_MAX = 0.5f; // seconds static const float HIST_STEP = 0.05f; // 20 hz static const float BIAS_MAX = 1e-1f; -static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; +static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP; static const size_t N_DIST_SUBS = 4; // for fault detection @@ -108,12 +108,13 @@ class BlockLocalPositionEstimator : public ModuleCommandstart(); diff --git a/apps/estimator/mag_bias_estimator/MagBiasEstimator.hpp b/apps/estimator/mag_bias_estimator/MagBiasEstimator.hpp index f849fc3503..ffa2265bbc 100644 --- a/apps/estimator/mag_bias_estimator/MagBiasEstimator.hpp +++ b/apps/estimator/mag_bias_estimator/MagBiasEstimator.hpp @@ -15,20 +15,21 @@ #include #include #include -// #include #include #include #include #include #include #include -#include #include #include #include #include #include +using namespace nextpilot; +using namespace nextpilot::global_params; + namespace mag_bias_estimator { class MagBiasEstimator : public ModuleCommand, public ModuleParams, public WorkItemScheduled { @@ -36,7 +37,7 @@ class MagBiasEstimator : public ModuleCommand, public ModulePa MagBiasEstimator(); ~MagBiasEstimator() override; - static int *instantiate(int argc, char *argv[]); + static MagBiasEstimator *instantiate(int argc, char *argv[]); /** @see ModuleCommand */ static int custom_command(int argc, char *argv[]) { diff --git a/apps/estimator/sensors/data_validator/SConscript b/apps/estimator/sensors/data_validator/SConscript index f569871ec3..c916b0ba62 100644 --- a/apps/estimator/sensors/data_validator/SConscript +++ b/apps/estimator/sensors/data_validator/SConscript @@ -7,6 +7,6 @@ cwd = GetCurrentDir() inc = [] src = Glob("*.cpp", exclude="*_test.cpp") -objs = DefineGroup("ins/sensors", src, depend=[""]) +objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSOR"]) Return("objs") diff --git a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 7287a14fa6..0e59eda30a 100644 --- a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -16,7 +16,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp index 340d4e00b0..1afe151f72 100644 --- a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp @@ -19,7 +19,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 66454e8bfd..71a60e66c2 100644 --- a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -19,7 +19,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 3e54ab0e51..05b8b90924 100644 --- a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -15,7 +15,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_imu/SConscript b/apps/estimator/sensors/vehicle_imu/SConscript index 892d65aac5..c916b0ba62 100644 --- a/apps/estimator/sensors/vehicle_imu/SConscript +++ b/apps/estimator/sensors/vehicle_imu/SConscript @@ -7,6 +7,6 @@ cwd = GetCurrentDir() inc = [] src = Glob("*.cpp", exclude="*_test.cpp") -objs = DefineGroup("ins/mag_bias_estimator", src, depend=["INS_USING_MAG_BIAS_ESTIMATOR"]) +objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSOR"]) Return("objs") diff --git a/apps/estimator/sensors/vehicle_imu/VehicleIMU.hpp b/apps/estimator/sensors/vehicle_imu/VehicleIMU.hpp index f51c5d204d..82e8a7219e 100644 --- a/apps/estimator/sensors/vehicle_imu/VehicleIMU.hpp +++ b/apps/estimator/sensors/vehicle_imu/VehicleIMU.hpp @@ -21,7 +21,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index d4efcfd282..cb78b5ee3b 100644 --- a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -22,7 +22,6 @@ #include #include -// #include #include #include #include diff --git a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 4ae56b5de5..a0de238677 100644 --- a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -21,7 +21,6 @@ #include #include #include -// #include #include #include #include diff --git a/apps/estimator/temperature_compensation/SConscript b/apps/estimator/temperature_compensation/SConscript index dbd6d09605..cdf51abb2a 100644 --- a/apps/estimator/temperature_compensation/SConscript +++ b/apps/estimator/temperature_compensation/SConscript @@ -7,6 +7,6 @@ cwd = GetCurrentDir() inc = [] src = Glob("*.cpp", exclude="*_test.cpp") + Glob("temperature_calibration/*.cpp") -objs = DefineGroup("ins/temperature_compensation", src, depend=["INS_USING_TEMPERATURE_COMPENSATION"]) +objs = DefineGroup("ins/temperature_compensation", src, depend=["_INS_USING_TEMPERATURE_COMPENSATION"]) Return("objs") diff --git a/apps/estimator/temperature_compensation/TemperatureCompensationModule.cpp b/apps/estimator/temperature_compensation/TemperatureCompensationModule.cpp index d556fadb99..fd15e9ea53 100644 --- a/apps/estimator/temperature_compensation/TemperatureCompensationModule.cpp +++ b/apps/estimator/temperature_compensation/TemperatureCompensationModule.cpp @@ -239,31 +239,32 @@ void TemperatureCompensationModule::Run() { perf_end(_loop_perf); } -int TemperatureCompensationModule::instantiate(int argc, char *argv[]) { +TemperatureCompensationModule *TemperatureCompensationModule::instantiate(int argc, char *argv[]) { TemperatureCompensationModule *instance = new TemperatureCompensationModule(); if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + // _object.store(instance); + // _task_id = task_id_is_work_queue; - if (instance->init()) { - return PX4_OK; - } + // if (instance->init()) { + // return PX4_OK; + // } + return instance; } else { PX4_ERR("alloc failed"); } delete instance; - _object.store(nullptr); - _task_id = -1; + // _object.store(nullptr); + // _task_id = -1; - return PX4_ERROR; + return nullptr; } -bool TemperatureCompensationModule::init() { +int TemperatureCompensationModule::init() { ScheduleOnInterval(1_s); - return true; + return 0; } int TemperatureCompensationModule::custom_command(int argc, char *argv[]) { @@ -276,7 +277,7 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[]) { int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { + while ((ch = getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': accel_calib = true; @@ -303,9 +304,11 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[]) { if (!is_running()) { PX4_WARN("background task not running"); - if (task_spawn(0, nullptr) != PX4_OK) { - return PX4_ERROR; - } + // if (task_spawn(0, nullptr) != PX4_OK) { + // return PX4_ERROR; + // } + + return 0; } vehicle_command_s vcmd{}; diff --git a/apps/estimator/temperature_compensation/TemperatureCompensationModule.h b/apps/estimator/temperature_compensation/TemperatureCompensationModule.h index 27df9f9f0a..d5d9636d6c 100644 --- a/apps/estimator/temperature_compensation/TemperatureCompensationModule.h +++ b/apps/estimator/temperature_compensation/TemperatureCompensationModule.h @@ -18,14 +18,9 @@ #include #include #include -#include -// #include #include -#include -#include #include #include -#include #include #include #include @@ -37,6 +32,8 @@ #include "TemperatureCompensation.h" using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace temperature_compensation { @@ -45,9 +42,6 @@ class TemperatureCompensationModule : public ModuleCommand { int finish(); private: - virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual inline int update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/baro.cpp b/apps/estimator/temperature_compensation/temperature_calibration/baro.cpp index 00e618b69e..1294f2a1d3 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/baro.cpp +++ b/apps/estimator/temperature_compensation/temperature_calibration/baro.cpp @@ -43,7 +43,7 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro() { } } -int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub) { +int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub) { bool finished = data.hot_soaked; bool updated; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/baro.h b/apps/estimator/temperature_compensation/temperature_calibration/baro.h index 32ce4b1522..1d50cbca83 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/baro.h +++ b/apps/estimator/temperature_compensation/temperature_calibration/baro.h @@ -26,7 +26,7 @@ class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFI int finish(); private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/common.h b/apps/estimator/temperature_compensation/temperature_calibration/common.h index c9d42bebdb..78ad415ced 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/common.h +++ b/apps/estimator/temperature_compensation/temperature_calibration/common.h @@ -12,18 +12,18 @@ #define TC_PRINT_DEBUG 0 #if TC_PRINT_DEBUG -#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); +# define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); #else -#define TC_DEBUG(fmt, ...) +# define TC_DEBUG(fmt, ...) #endif #include #include #include - +#include #include "polyfit.hpp" -#define SENSOR_COUNT_MAX 3 +#define SENSOR_COUNT_MAX 3 #define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature #define TC_ERROR_COMMUNICATION 112 ///< no sensors found @@ -140,10 +140,10 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase { struct PerSensorData { float sensor_sample_filt[Dim + 1]; ///< last value is the temperature polyfitter P[Dim]; - unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces + unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces from the last reading */ - uint32_t device_id = 0; ///< ID for the sensor being calibrated - bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been + uint32_t device_id = 0; ///< ID for the sensor being calibrated + bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been /// verified and the starting temperature set bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase bool tempcal_complete = false; ///< true when the calibration has been completed @@ -160,8 +160,8 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase { * update a single sensor instance * @return 0 when done, 1 not finished yet, <0 for an error */ - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; + virtual int update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub) = 0; - unsigned _num_sensor_instances{0}; - int _sensor_subs[SENSOR_COUNT_MAX]; + unsigned _num_sensor_instances{0}; + orb_subscr_t _sensor_subs[SENSOR_COUNT_MAX]; }; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/gyro.cpp b/apps/estimator/temperature_compensation/temperature_calibration/gyro.cpp index 969b8d57f5..70cdc063c1 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/gyro.cpp +++ b/apps/estimator/temperature_compensation/temperature_calibration/gyro.cpp @@ -22,7 +22,7 @@ #include TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, - float max_start_temperature, int gyro_subs[], int num_gyros) : + float max_start_temperature, orb_subscr_t gyro_subs[], int num_gyros) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { for (int i = 0; i < num_gyros; ++i) { _sensor_subs[i] = gyro_subs[i]; @@ -31,7 +31,7 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris _num_sensor_instances = num_gyros; } -int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub) { +int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub) { bool finished = data.hot_soaked; bool updated; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/gyro.h b/apps/estimator/temperature_compensation/temperature_calibration/gyro.h index eace338fc9..e5df73f4bd 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/gyro.h +++ b/apps/estimator/temperature_compensation/temperature_calibration/gyro.h @@ -16,7 +16,8 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, - int gyro_subs[], int num_gyros); + orb_subscr_t gyro_subs[], int num_gyros); + virtual ~TemperatureCalibrationGyro() { } @@ -26,7 +27,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { int finish(); private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_subscr_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/apps/estimator/temperature_compensation/temperature_calibration/polyfit.hpp b/apps/estimator/temperature_compensation/temperature_calibration/polyfit.hpp index 509316774e..c00c2792da 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/polyfit.hpp +++ b/apps/estimator/temperature_compensation/temperature_calibration/polyfit.hpp @@ -65,11 +65,8 @@ Author: Siddharth Bharat Purohit */ #pragma once -// #include #include -#include -#include -#include + #include @@ -77,9 +74,9 @@ Author: Siddharth Bharat Purohit #define DEBUG 0 #if DEBUG -#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); +# define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__); #else -#define PF_DEBUG(fmt, ...) +# define PF_DEBUG(fmt, ...) #endif template @@ -130,7 +127,7 @@ class polyfitter { for (int i = _forder - 1; i >= 0; i--) { _VTY(i) += y * temp; - temp *= x; + temp *= x; PF_DEBUG("%.6f ", (double)_VTY(i)); } @@ -158,8 +155,8 @@ class polyfitter { } for (int j = i - z; j >= z; j--) { - int row = j; - int col = i - j; + int row = j; + int col = i - j; _VTV(row, col) += (double)temp; } diff --git a/apps/estimator/temperature_compensation/temperature_calibration/task.cpp b/apps/estimator/temperature_compensation/temperature_calibration/task.cpp index ab3785ef94..87c37ccc59 100644 --- a/apps/estimator/temperature_compensation/temperature_calibration/task.cpp +++ b/apps/estimator/temperature_compensation/temperature_calibration/task.cpp @@ -21,8 +21,8 @@ #include #include #include -#include -#include + + #include #include @@ -45,6 +45,7 @@ class TemperatureCalibration { TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) { } + ~TemperatureCalibration() = default; /** @@ -70,9 +71,9 @@ class TemperatureCalibration { bool _force_task_exit = false; int _control_task = -1; // task handle for task - const bool _accel; ///< enable accel calibration? - const bool _baro; ///< enable baro calibration? - const bool _gyro; ///< enable gyro calibration? + const bool _accel; ///< enable accel calibration? + const bool _baro; ///< enable baro calibration? + const bool _gyro; ///< enable gyro calibration? }; void TemperatureCalibration::task_main() { diff --git a/apps/libraries/common/console_buffer/console_buffer_usr.cpp b/apps/libraries/common/console_buffer/console_buffer_usr.cpp index 63ff93f284..bcdf1bbb81 100644 --- a/apps/libraries/common/console_buffer/console_buffer_usr.cpp +++ b/apps/libraries/common/console_buffer/console_buffer_usr.cpp @@ -31,11 +31,9 @@ * ****************************************************************************/ -// #include -#include +#include #include -#include #include #include #include diff --git a/apps/libraries/common/mathlib/math/WelfordMeanVector.hpp b/apps/libraries/common/mathlib/math/WelfordMeanVector.hpp index 8f55eca5ba..3305eece2e 100644 --- a/apps/libraries/common/mathlib/math/WelfordMeanVector.hpp +++ b/apps/libraries/common/mathlib/math/WelfordMeanVector.hpp @@ -16,6 +16,8 @@ #pragma once +#include + namespace math { diff --git a/apps/libraries/controller/adsb/AdsbConflict.h b/apps/libraries/controller/adsb/AdsbConflict.h index 838a836829..46333f8046 100644 --- a/apps/libraries/controller/adsb/AdsbConflict.h +++ b/apps/libraries/controller/adsb/AdsbConflict.h @@ -22,7 +22,6 @@ #include #include #include -// #include #include using namespace time_literals; diff --git a/apps/libraries/controller/rc/dsm.cpp b/apps/libraries/controller/rc/dsm.cpp index cec13a5d14..40cafa87e5 100644 --- a/apps/libraries/controller/rc/dsm.cpp +++ b/apps/libraries/controller/rc/dsm.cpp @@ -16,7 +16,7 @@ * Decodes into the global PPM buffer and updates accordingly. */ -//#include + #include #include #include @@ -31,7 +31,7 @@ #include "common_rc.h" #include -#include +#include #if defined(__PX4_NUTTX) #include diff --git a/apps/libraries/controller/rc/dsm.h b/apps/libraries/controller/rc/dsm.h index 7f876d7203..2f7180b67a 100644 --- a/apps/libraries/controller/rc/dsm.h +++ b/apps/libraries/controller/rc/dsm.h @@ -20,7 +20,7 @@ #include -//#include + #include __BEGIN_DECLS diff --git a/apps/libraries/controller/rc/sbus.cpp b/apps/libraries/controller/rc/sbus.cpp index 29934e3267..42164f3d65 100644 --- a/apps/libraries/controller/rc/sbus.cpp +++ b/apps/libraries/controller/rc/sbus.cpp @@ -14,7 +14,7 @@ * Serial protocol decoder for the Futaba S.bus protocol. */ -//#include + #include #include diff --git a/apps/libraries/estimator/sensor_calibration/Accelerometer.hpp b/apps/libraries/estimator/sensor_calibration/Accelerometer.hpp index 8d76d40f81..fc1080f0bc 100644 --- a/apps/libraries/estimator/sensor_calibration/Accelerometer.hpp +++ b/apps/libraries/estimator/sensor_calibration/Accelerometer.hpp @@ -12,7 +12,7 @@ #include #include -//#include + #include #include #include diff --git a/apps/libraries/estimator/sensor_calibration/Barometer.hpp b/apps/libraries/estimator/sensor_calibration/Barometer.hpp index 457d89b72a..3ab0bf69e2 100644 --- a/apps/libraries/estimator/sensor_calibration/Barometer.hpp +++ b/apps/libraries/estimator/sensor_calibration/Barometer.hpp @@ -10,7 +10,7 @@ #pragma once -//#include + #include #include #include diff --git a/apps/libraries/estimator/sensor_calibration/Gyroscope.hpp b/apps/libraries/estimator/sensor_calibration/Gyroscope.hpp index 9a1808cabd..3137849621 100644 --- a/apps/libraries/estimator/sensor_calibration/Gyroscope.hpp +++ b/apps/libraries/estimator/sensor_calibration/Gyroscope.hpp @@ -12,7 +12,6 @@ #include #include -//#include #include #include #include diff --git a/apps/libraries/estimator/sensor_calibration/Magnetometer.hpp b/apps/libraries/estimator/sensor_calibration/Magnetometer.hpp index bc45c68f3f..116c08a464 100644 --- a/apps/libraries/estimator/sensor_calibration/Magnetometer.hpp +++ b/apps/libraries/estimator/sensor_calibration/Magnetometer.hpp @@ -12,7 +12,7 @@ #include #include -//#include + #include #include #include diff --git a/apps/libraries/peripheral/battery/battery/battery.h b/apps/libraries/peripheral/battery/battery/battery.h index e23ce43112..82685e112d 100644 --- a/apps/libraries/peripheral/battery/battery/battery.h +++ b/apps/libraries/peripheral/battery/battery/battery.h @@ -23,7 +23,6 @@ #include // #include -// #include #include #include diff --git a/apps/libraries/peripheral/battery/smbus_sbs/SBS.hpp b/apps/libraries/peripheral/battery/smbus_sbs/SBS.hpp index 875f15b5ab..7782707093 100644 --- a/apps/libraries/peripheral/battery/smbus_sbs/SBS.hpp +++ b/apps/libraries/peripheral/battery/smbus_sbs/SBS.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include diff --git a/apps/libraries/peripheral/drivers/drv_hrt.h b/apps/libraries/peripheral/drivers/drv_hrt.h index 93020b5f24..727435d61c 100644 --- a/apps/libraries/peripheral/drivers/drv_hrt.h +++ b/apps/libraries/peripheral/drivers/drv_hrt.h @@ -22,8 +22,7 @@ // #include // #include // #include -// // #include -// #include +// // #include // #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) // #include diff --git a/apps/libraries/peripheral/drivers/led/led.cpp b/apps/libraries/peripheral/drivers/led/led.cpp index a7e9f3904a..6276db7a78 100644 --- a/apps/libraries/peripheral/drivers/led/led.cpp +++ b/apps/libraries/peripheral/drivers/led/led.cpp @@ -15,7 +15,6 @@ */ #include -//#include #include #include #include diff --git a/apps/libraries/peripheral/rcinput/rc/crsf.cpp b/apps/libraries/peripheral/rcinput/rc/crsf.cpp index d52e12d58b..7efebe0422 100644 --- a/apps/libraries/peripheral/rcinput/rc/crsf.cpp +++ b/apps/libraries/peripheral/rcinput/rc/crsf.cpp @@ -20,7 +20,7 @@ #define CRSF_VERBOSE(...) #endif -#include +#include #include #include #include diff --git a/apps/libraries/peripheral/rcinput/rc/crsf.h b/apps/libraries/peripheral/rcinput/rc/crsf.h index f670627da2..11c78960fd 100644 --- a/apps/libraries/peripheral/rcinput/rc/crsf.h +++ b/apps/libraries/peripheral/rcinput/rc/crsf.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include __BEGIN_DECLS diff --git a/apps/libraries/peripheral/rcinput/rc/dsm.cpp b/apps/libraries/peripheral/rcinput/rc/dsm.cpp index da1f0d691a..a74181176d 100644 --- a/apps/libraries/peripheral/rcinput/rc/dsm.cpp +++ b/apps/libraries/peripheral/rcinput/rc/dsm.cpp @@ -16,10 +16,10 @@ * Decodes into the global PPM buffer and updates accordingly. */ -#include + #include -#include -#include +#include +#include #include #include @@ -30,7 +30,7 @@ #include "dsm.h" #include "spektrum_rssi.h" #include "common_rc.h" -#include +#include #include diff --git a/apps/libraries/peripheral/rcinput/rc/dsm.h b/apps/libraries/peripheral/rcinput/rc/dsm.h index 0a709357fd..2f7180b67a 100644 --- a/apps/libraries/peripheral/rcinput/rc/dsm.h +++ b/apps/libraries/peripheral/rcinput/rc/dsm.h @@ -20,8 +20,8 @@ #include -#include -#include + +#include __BEGIN_DECLS diff --git a/apps/libraries/peripheral/rcinput/rc/ghst.cpp b/apps/libraries/peripheral/rcinput/rc/ghst.cpp index 7e2e9e2218..035d003c2f 100644 --- a/apps/libraries/peripheral/rcinput/rc/ghst.cpp +++ b/apps/libraries/peripheral/rcinput/rc/ghst.cpp @@ -29,7 +29,7 @@ #define GHST_VERBOSE(...) #endif -#include +#include #include #include #include diff --git a/apps/libraries/peripheral/rcinput/rc/ghst.hpp b/apps/libraries/peripheral/rcinput/rc/ghst.hpp index 859e8b6ec8..2434625fd2 100644 --- a/apps/libraries/peripheral/rcinput/rc/ghst.hpp +++ b/apps/libraries/peripheral/rcinput/rc/ghst.hpp @@ -20,7 +20,7 @@ #pragma once #include -#include +#include __BEGIN_DECLS diff --git a/apps/libraries/peripheral/rcinput/rc/rc_tests/RCTest.cpp b/apps/libraries/peripheral/rcinput/rc/rc_tests/RCTest.cpp index 07c45d33c0..2c21ecc4fe 100644 --- a/apps/libraries/peripheral/rcinput/rc/rc_tests/RCTest.cpp +++ b/apps/libraries/peripheral/rcinput/rc/rc_tests/RCTest.cpp @@ -6,15 +6,15 @@ #include #include -#include +#include #define DSM_DEBUG -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define TEST_DATA_PATH "./test_data/" @@ -525,4 +525,3 @@ bool RCTest::sumdTest() ut_declare_test_c(rc_tests_main, RCTest) - diff --git a/apps/libraries/peripheral/rcinput/rc/sbus.cpp b/apps/libraries/peripheral/rcinput/rc/sbus.cpp index 08540aa02e..e5166d93b7 100644 --- a/apps/libraries/peripheral/rcinput/rc/sbus.cpp +++ b/apps/libraries/peripheral/rcinput/rc/sbus.cpp @@ -14,7 +14,6 @@ * Serial protocol decoder for the Futaba S.bus protocol. */ -#include #include #include @@ -26,8 +25,8 @@ #include "sbus.h" #include "common_rc.h" -#include -#include +#include +#include using namespace time_literals; diff --git a/apps/libraries/telemetry/button/ButtonPublisher.hpp b/apps/libraries/telemetry/button/ButtonPublisher.hpp index 77287835a0..073af788e4 100644 --- a/apps/libraries/telemetry/button/ButtonPublisher.hpp +++ b/apps/libraries/telemetry/button/ButtonPublisher.hpp @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/apps/libraries/telemetry/timesync/Timesync.hpp b/apps/libraries/telemetry/timesync/Timesync.hpp index 496792e3c8..ef83aa0475 100644 --- a/apps/libraries/telemetry/timesync/Timesync.hpp +++ b/apps/libraries/telemetry/timesync/Timesync.hpp @@ -17,7 +17,7 @@ #pragma once -#include +#include #include #include diff --git a/apps/peripheral/battery/battery_status/analog_battery.cpp b/apps/peripheral/battery/battery_status/analog_battery.cpp index 05473b2a71..0aa7c29503 100644 --- a/apps/peripheral/battery/battery_status/analog_battery.cpp +++ b/apps/peripheral/battery/battery_status/analog_battery.cpp @@ -9,17 +9,17 @@ ******************************************************************/ #include -#include +#include #include "analog_battery.h" // Defaults to use if the parameters are not set #if BOARD_NUMBER_BRICKS > 0 -#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST) +# if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST) static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST; static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST; -#else -#error "BOARD_BATT_V_LIST and BOARD_BATT_I_LIST need to be defined" -#endif +# else +# error "BOARD_BATT_V_LIST and BOARD_BATT_I_LIST need to be defined" +# endif #else static constexpr int DEFAULT_V_CHANNEL[1] = {-1}; static constexpr int DEFAULT_I_CHANNEL[1] = {-1}; @@ -50,8 +50,7 @@ void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_ const float voltage_v = voltage_raw * _analog_params.v_div; const float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; - const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && - (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); Battery::setConnected(connected); Battery::updateVoltage(voltage_v); diff --git a/apps/peripheral/battery/battery_status/battery_status.cpp b/apps/peripheral/battery/battery_status/battery_status.cpp index 6d83944c0c..09f4d5803f 100644 --- a/apps/peripheral/battery/battery_status/battery_status.cpp +++ b/apps/peripheral/battery/battery_status/battery_status.cpp @@ -18,27 +18,24 @@ * @author Beat Küng */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include -#include +#include + #include "analog_battery.h" @@ -49,11 +46,11 @@ using namespace time_literals; */ #ifndef BOARD_NUMBER_BRICKS -#error "battery_status module requires power bricks" +# error "battery_status module requires power bricks" #endif #if BOARD_NUMBER_BRICKS == 0 -#error "battery_status module requires power bricks" +# error "battery_status module requires power bricks" #endif class BatteryStatus : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -92,7 +89,7 @@ class BatteryStatus : public ModuleBase, public ModuleParams, pub #if BOARD_NUMBER_BRICKS > 1 &_battery2, #endif - }; // End _analogBatteries + } // End _analogBatteries perf_counter_t _loop_perf; /**< loop performance counter */ @@ -173,15 +170,11 @@ void BatteryStatus::adc_poll() { if (adc_report.channel_id[i] >= 0) { if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) { /* Voltage in volts */ - bat_voltage_adc_readings[b] = adc_report.raw_data[i] * - adc_report.v_ref / - adc_report.resolution; + bat_voltage_adc_readings[b] = adc_report.raw_data[i] * adc_report.v_ref / adc_report.resolution; has_bat_voltage_adc_channel[b] = true; } else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) { - bat_current_adc_readings[b] = adc_report.raw_data[i] * - adc_report.v_ref / - adc_report.resolution; + bat_current_adc_readings[b] = adc_report.raw_data[i] * adc_report.v_ref / adc_report.resolution; } } } diff --git a/apps/peripheral/battery/esc_battery/EscBattery.hpp b/apps/peripheral/battery/esc_battery/EscBattery.hpp index 27ee9d5d20..c72f6ac00f 100644 --- a/apps/peripheral/battery/esc_battery/EscBattery.hpp +++ b/apps/peripheral/battery/esc_battery/EscBattery.hpp @@ -10,11 +10,10 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/apps/peripheral/payload/camera_feedback/CameraFeedback.hpp b/apps/peripheral/payload/camera_feedback/CameraFeedback.hpp index acd76edae8..0ccf368d2a 100644 --- a/apps/peripheral/payload/camera_feedback/CameraFeedback.hpp +++ b/apps/peripheral/payload/camera_feedback/CameraFeedback.hpp @@ -17,17 +17,16 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include + +#include +#include +#include + +#include #include #include -#include #include #include #include diff --git a/apps/peripheral/payload/gimbal/gimbal.cpp b/apps/peripheral/payload/gimbal/gimbal.cpp index 46bd574f25..f3582b7aee 100644 --- a/apps/peripheral/payload/gimbal/gimbal.cpp +++ b/apps/peripheral/payload/gimbal/gimbal.cpp @@ -24,8 +24,8 @@ #include #include #include -#include -#include +#include + #include "gimbal_params.h" #include "input_mavlink.h" @@ -38,8 +38,8 @@ #include #include -#include -#include +#include +#include using namespace time_literals; using namespace gimbal; @@ -57,6 +57,7 @@ struct ThreadData { InputTest *test_input = nullptr; ControlData control_data{}; }; + static ThreadData *g_thread_data = nullptr; static void usage(); @@ -143,21 +144,27 @@ static int gimbal_thread_main(int argc, char *argv[]) { case 0: // AUX thread_data.output_obj = new OutputRC(params); - if (!thread_data.output_obj) { alloc_failed = true; } + if (!thread_data.output_obj) { + alloc_failed = true; + } break; case 1: // MAVLink gimbal v1 protocol thread_data.output_obj = new OutputMavlinkV1(params); - if (!thread_data.output_obj) { alloc_failed = true; } + if (!thread_data.output_obj) { + alloc_failed = true; + } break; case 2: // MAVLink gimbal v2 protocol thread_data.output_obj = new OutputMavlinkV2(params); - if (!thread_data.output_obj) { alloc_failed = true; } + if (!thread_data.output_obj) { + alloc_failed = true; + } break; @@ -485,27 +492,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) { param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN"); param_handles.mnt_lnd_p_max = param_find("MNT_LND_P_MAX"); - if (param_handles.mnt_mode_in == PARAM_INVALID || - param_handles.mnt_mode_out == PARAM_INVALID || - param_handles.mnt_mav_sysid_v1 == PARAM_INVALID || - param_handles.mnt_mav_compid_v1 == PARAM_INVALID || - param_handles.mnt_man_pitch == PARAM_INVALID || - param_handles.mnt_man_roll == PARAM_INVALID || - param_handles.mnt_man_yaw == PARAM_INVALID || - param_handles.mnt_do_stab == PARAM_INVALID || - param_handles.mnt_range_pitch == PARAM_INVALID || - param_handles.mnt_range_roll == PARAM_INVALID || - param_handles.mnt_range_yaw == PARAM_INVALID || - param_handles.mnt_off_pitch == PARAM_INVALID || - param_handles.mnt_off_roll == PARAM_INVALID || - param_handles.mnt_off_yaw == PARAM_INVALID || - param_handles.mav_sysid == PARAM_INVALID || - param_handles.mav_compid == PARAM_INVALID || - param_handles.mnt_rate_pitch == PARAM_INVALID || - param_handles.mnt_rate_yaw == PARAM_INVALID || - param_handles.mnt_rc_in_mode == PARAM_INVALID || - param_handles.mnt_lnd_p_min == PARAM_INVALID || - param_handles.mnt_lnd_p_max == PARAM_INVALID) { + if (param_handles.mnt_mode_in == PARAM_INVALID || param_handles.mnt_mode_out == PARAM_INVALID || param_handles.mnt_mav_sysid_v1 == PARAM_INVALID || param_handles.mnt_mav_compid_v1 == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || param_handles.mnt_man_roll == PARAM_INVALID || param_handles.mnt_man_yaw == PARAM_INVALID || param_handles.mnt_do_stab == PARAM_INVALID || param_handles.mnt_range_pitch == PARAM_INVALID || param_handles.mnt_range_roll == PARAM_INVALID || param_handles.mnt_range_yaw == PARAM_INVALID || param_handles.mnt_off_pitch == PARAM_INVALID || param_handles.mnt_off_roll == PARAM_INVALID || param_handles.mnt_off_yaw == PARAM_INVALID || param_handles.mav_sysid == PARAM_INVALID || param_handles.mav_compid == PARAM_INVALID || param_handles.mnt_rate_pitch == PARAM_INVALID || param_handles.mnt_rate_yaw == PARAM_INVALID || param_handles.mnt_rc_in_mode == PARAM_INVALID || param_handles.mnt_lnd_p_min == PARAM_INVALID || param_handles.mnt_lnd_p_max == PARAM_INVALID) { return false; } diff --git a/apps/peripheral/payload/gimbal/gimbal_params.h b/apps/peripheral/payload/gimbal/gimbal_params.h index 8f7ac2ae9c..c67f2fdaa4 100644 --- a/apps/peripheral/payload/gimbal/gimbal_params.h +++ b/apps/peripheral/payload/gimbal/gimbal_params.h @@ -11,7 +11,7 @@ #pragma once #include -#include +#include namespace gimbal { diff --git a/apps/peripheral/payload/gimbal/input_mavlink.cpp b/apps/peripheral/payload/gimbal/input_mavlink.cpp index 39a564ac80..1fbfbaf679 100644 --- a/apps/peripheral/payload/gimbal/input_mavlink.cpp +++ b/apps/peripheral/payload/gimbal/input_mavlink.cpp @@ -12,9 +12,9 @@ #include #include #include -#include -#include -#include +#include +#include + #include #include #include @@ -206,8 +206,7 @@ InputMavlinkCmdMount::UpdateResult InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_command_s &vehicle_command) { // Process only if the command is for us or for anyone (component id 0). const bool sysid_correct = (vehicle_command.target_system == _parameters.mav_sysid); - const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || - (vehicle_command.target_component == 0)); + const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || (vehicle_command.target_component == 0)); if (!sysid_correct || !compid_correct) { return UpdateResult::NoUpdate; @@ -417,13 +416,7 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData gimbal_manager_info.timestamp = hrt_absolute_time(); gimbal_manager_info.cap_flags = - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; gimbal_manager_info.pitch_max = M_PI_F / 2; gimbal_manager_info.pitch_min = -M_PI_F / 2; @@ -534,8 +527,7 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(ControlData &control_data, const gimbal_manager_set_attitude_s &set_attitude) { - if (set_attitude.origin_sysid == control_data.sysid_primary_control && - set_attitude.origin_compid == control_data.compid_primary_control) { + if (set_attitude.origin_sysid == control_data.sysid_primary_control && set_attitude.origin_compid == control_data.compid_primary_control) { const matrix::Quatf q(set_attitude.q); const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, set_attitude.angular_velocity_y, @@ -605,8 +597,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ // Process only if the command is for us or for anyone (component id 0). const bool sysid_correct = (vehicle_command.target_system == _parameters.mav_sysid) || (vehicle_command.target_system == 0); - const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || - (vehicle_command.target_component == 0)); + const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || (vehicle_command.target_component == 0)); if (!sysid_correct || !compid_correct) { return UpdateResult::NoUpdate; @@ -702,8 +693,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); return UpdateResult::UpdatedActive; - } else if (vehicle_command.command == - vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { const int param_sysid = roundf(vehicle_command.param1); const int param_compid = roundf(vehicle_command.param2); @@ -765,8 +755,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - if (new_sysid_primary_control != control_data.sysid_primary_control || - new_compid_primary_control != control_data.compid_primary_control) { + if (new_sysid_primary_control != control_data.sysid_primary_control || new_compid_primary_control != control_data.compid_primary_control) { PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", control_data.sysid_primary_control, control_data.compid_primary_control, new_sysid_primary_control, new_compid_primary_control); @@ -779,10 +768,8 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ // TODO: support secondary control // TODO: support gimbal device id for multiple gimbals - } else if (vehicle_command.command == - vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { - if (vehicle_command.source_system == control_data.sysid_primary_control && - vehicle_command.source_component == control_data.compid_primary_control) { + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { + if (vehicle_command.source_system == control_data.sysid_primary_control && vehicle_command.source_component == control_data.compid_primary_control) { const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); const matrix::Quatf q(euler); @@ -815,21 +802,17 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_control(ControlData &control_data, const gimbal_manager_set_manual_control_s &set_manual_control) { - if (set_manual_control.origin_sysid == control_data.sysid_primary_control && - set_manual_control.origin_compid == control_data.compid_primary_control) { + if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) : - matrix::Quatf(NAN, NAN, NAN, NAN); + (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? matrix::Quatf( + matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) + : matrix::Quatf(NAN, NAN, NAN, NAN); const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? matrix::Vector3f(0.0f, + math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, + math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) + : matrix::Vector3f(NAN, NAN, NAN); _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, angular_velocity); diff --git a/apps/peripheral/payload/gimbal/input_mavlink.h b/apps/peripheral/payload/gimbal/input_mavlink.h index e02887bace..591fc99e98 100644 --- a/apps/peripheral/payload/gimbal/input_mavlink.h +++ b/apps/peripheral/payload/gimbal/input_mavlink.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include namespace gimbal { diff --git a/apps/peripheral/payload/gimbal/input_rc.cpp b/apps/peripheral/payload/gimbal/input_rc.cpp index 61891482b5..cd0705d1a3 100644 --- a/apps/peripheral/payload/gimbal/input_rc.cpp +++ b/apps/peripheral/payload/gimbal/input_rc.cpp @@ -14,9 +14,9 @@ #include #include #include -#include -#include -#include + +#include +#include namespace gimbal { diff --git a/apps/peripheral/payload/gimbal/input_test.cpp b/apps/peripheral/payload/gimbal/input_test.cpp index eef738ad68..b6e8785535 100644 --- a/apps/peripheral/payload/gimbal/input_test.cpp +++ b/apps/peripheral/payload/gimbal/input_test.cpp @@ -10,10 +10,10 @@ #include "input_test.h" -#include -#include -#include -#include + +#include +#include +#include namespace gimbal { diff --git a/apps/peripheral/payload/gimbal/input_test.h b/apps/peripheral/payload/gimbal/input_test.h index 1a8a79c625..e04982503b 100644 --- a/apps/peripheral/payload/gimbal/input_test.h +++ b/apps/peripheral/payload/gimbal/input_test.h @@ -11,7 +11,7 @@ #pragma once #include "input.h" -#include +#include namespace gimbal { diff --git a/apps/peripheral/payload/gimbal/output.cpp b/apps/peripheral/payload/gimbal/output.cpp index ea47bd74bc..c21e395d3f 100644 --- a/apps/peripheral/payload/gimbal/output.cpp +++ b/apps/peripheral/payload/gimbal/output.cpp @@ -13,8 +13,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -124,9 +124,7 @@ void OutputBase::_handle_position_update(const ControlData &control_data, bool f float roll = PX4_ISFINITE(control_data.type_data.lonlat.roll_offset) ? control_data.type_data.lonlat.roll_offset : 0.0f; // interface: use fixed pitch value > -pi otherwise consider ROI altitude - float pitch = (control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ? - control_data.type_data.lonlat.pitch_fixed_angle : - _calculate_pitch(lon, lat, alt, vehicle_global_position); + float pitch = (control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ? control_data.type_data.lonlat.pitch_fixed_angle : _calculate_pitch(lon, lat, alt, vehicle_global_position); float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon); // We set the yaw angle in the absolute frame in this case. diff --git a/apps/peripheral/payload/gimbal/output.h b/apps/peripheral/payload/gimbal/output.h index 1e8dc65e2d..61f1fedd71 100644 --- a/apps/peripheral/payload/gimbal/output.h +++ b/apps/peripheral/payload/gimbal/output.h @@ -12,8 +12,8 @@ #include "common.h" #include "gimbal_params.h" -#include -#include +#include +#include #include #include #include diff --git a/apps/peripheral/payload/gimbal/output_mavlink.cpp b/apps/peripheral/payload/gimbal/output_mavlink.cpp index 0dc0e289d0..75af988e7d 100644 --- a/apps/peripheral/payload/gimbal/output_mavlink.cpp +++ b/apps/peripheral/payload/gimbal/output_mavlink.cpp @@ -9,10 +9,8 @@ ******************************************************************/ #include "output_mavlink.h" - #include - -#include +#include namespace gimbal { @@ -91,9 +89,7 @@ void OutputMavlinkV1::_stream_device_attitude_status() { attitude_status.timestamp = hrt_absolute_time(); attitude_status.target_system = 0; attitude_status.target_component = 0; - attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | - gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | - gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); matrix::Quatf q(euler); diff --git a/apps/peripheral/payload/gimbal/output_rc.cpp b/apps/peripheral/payload/gimbal/output_rc.cpp index 69eef8f39d..8917385c74 100644 --- a/apps/peripheral/payload/gimbal/output_rc.cpp +++ b/apps/peripheral/payload/gimbal/output_rc.cpp @@ -11,7 +11,7 @@ #include "output_rc.h" #include -#include +#include #include using math::constrain; @@ -39,16 +39,13 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( - (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * - (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), + (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), -1.f, 1.f); gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = constrain( - (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * - (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), + (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), -1.f, 1.f); gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = constrain( - (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * - (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), + (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), -1.f, 1.f); gimbal_controls.timestamp = hrt_absolute_time(); _gimbal_controls_pub.publish(gimbal_controls); @@ -65,10 +62,7 @@ void OutputRC::_stream_device_attitude_status() { attitude_status.timestamp = hrt_absolute_time(); attitude_status.target_system = 0; attitude_status.target_component = 0; - attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | - gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | - gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | - gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); matrix::Quatf q(euler); diff --git a/apps/peripheral/payload/payload_deliverer/gripper.h b/apps/peripheral/payload/payload_deliverer/gripper.h index 94e7abfc00..94cf49940c 100644 --- a/apps/peripheral/payload/payload_deliverer/gripper.h +++ b/apps/peripheral/payload/payload_deliverer/gripper.h @@ -8,7 +8,7 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include +#include #include #include diff --git a/apps/peripheral/payload/payload_deliverer/payload_deliverer.h b/apps/peripheral/payload/payload_deliverer/payload_deliverer.h index 0425780790..e55c22131a 100644 --- a/apps/peripheral/payload/payload_deliverer/payload_deliverer.h +++ b/apps/peripheral/payload/payload_deliverer/payload_deliverer.h @@ -14,10 +14,11 @@ */ #include "gripper.h" -#include -#include -#include -#include +#include +#include +#include +#include + #include #include diff --git a/apps/simulation/gz_bridge/GZBridge.hpp b/apps/simulation/gz_bridge/GZBridge.hpp index f995144e76..bbe4f578da 100644 --- a/apps/simulation/gz_bridge/GZBridge.hpp +++ b/apps/simulation/gz_bridge/GZBridge.hpp @@ -17,12 +17,11 @@ #include #include #include -#include + #include #include #include #include -#include #include #include #include diff --git a/apps/simulation/simulator_mavlink/SimulatorMavlink.cpp b/apps/simulation/simulator_mavlink/SimulatorMavlink.cpp index a8e36bc8f9..a7eec19cb8 100644 --- a/apps/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/apps/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -11,8 +11,8 @@ #include "SimulatorMavlink.hpp" #include -#include -#include + + #include #include #include @@ -96,9 +96,9 @@ void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_contr } } - msg->mode = mode_flag_custom; - msg->mode |= (armed) ? mode_flag_armed : 0; - msg->flags = 0; + msg->mode = mode_flag_custom; + msg->mode |= (armed) ? mode_flag_armed : 0; + msg->flags = 0; #if defined(ENABLE_LOCKSTEP_SCHEDULER) msg->flags |= 1; @@ -118,13 +118,12 @@ void SimulatorMavlink::send_esc_telemetry(mavlink_hil_actuator_controls_t hil_ac max_esc_index = i; } - esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... + esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... esc_status.esc[i].timestamp = esc_status.timestamp; - esc_status.esc[i].esc_errorcount = 0; // TODO + esc_status.esc[i].esc_errorcount = 0; // TODO esc_status.esc[i].esc_voltage = _battery_status.voltage_v; - esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f : - 0.0f; // TODO: magic number - esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number + esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f : 0.0f; // TODO: magic number + esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number esc_status.esc[i].esc_temperature = 20.0 + math::abs_t(hil_act_control.controls[i]) * 40.0; } @@ -378,8 +377,8 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) { gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m - gps.hdop = 0; // TODO - gps.vdop = 0; // TODO + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO gps.noise_per_ms = 0; gps.automatic_gain_control = 0; @@ -892,9 +891,9 @@ void SimulatorMavlink::handle_message_vision_position_estimate(const mavlink_mes // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). // If unknown, assign NaN value to first element in the array. - odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0 - odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1 - odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2 + odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0 + odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1 + odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2 odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3 odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4 @@ -995,10 +994,10 @@ void SimulatorMavlink::request_hil_state_quaternion() { } void SimulatorMavlink::send_heartbeat() { - mavlink_heartbeat_t hb = {}; - mavlink_message_t message = {}; - hb.autopilot = 12; - hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; + mavlink_heartbeat_t hb = {}; + mavlink_message_t message = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; mavlink_msg_heartbeat_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hb); send_mavlink_message(message); } @@ -1011,6 +1010,7 @@ void SimulatorMavlink::run() { #endif struct sockaddr_in _myaddr {}; + _myaddr.sin_family = AF_INET; _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); _myaddr.sin_port = htons(_port); @@ -1396,9 +1396,7 @@ void SimulatorMavlink::check_failure_injections() { vehicle_command_ack_s ack{}; ack.command = vehicle_command.command; ack.from_external = false; - ack.result = supported ? - vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : - vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.result = supported ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; ack.timestamp = hrt_absolute_time(); _command_ack_pub.publish(ack); } diff --git a/apps/simulation/simulator_mavlink/SimulatorMavlink.hpp b/apps/simulation/simulator_mavlink/SimulatorMavlink.hpp index 30d34cab0a..a65dd84931 100644 --- a/apps/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/apps/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -26,7 +26,7 @@ #include #include #include -#include + #include #include #include @@ -82,8 +82,7 @@ static inline SensorSource operator&(A lhs, B rhs) { typedef typename std::underlying_type::type underlying; return static_cast( - static_cast(lhs) & - static_cast(rhs)); + static_cast(lhs) & static_cast(rhs)); } class SimulatorMavlink : public ModuleParams { @@ -102,12 +101,15 @@ class SimulatorMavlink : public ModuleParams { void set_ip(InternetProtocol ip) { _ip = ip; } + void set_port(unsigned port) { _port = port; } + void set_hostname(const char *hostname) { _hostname = hostname; } + void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; } diff --git a/apps/storage/logger/log_writer_file.cpp b/apps/storage/logger/log_writer_file.cpp index a501bea832..2608f93d9f 100644 --- a/apps/storage/logger/log_writer_file.cpp +++ b/apps/storage/logger/log_writer_file.cpp @@ -14,7 +14,7 @@ #include #include #include -// #include + // #include #include diff --git a/apps/storage/logger/watchdog.cpp b/apps/storage/logger/watchdog.cpp index 966ab9ca2f..069685fdef 100644 --- a/apps/storage/logger/watchdog.cpp +++ b/apps/storage/logger/watchdog.cpp @@ -11,7 +11,7 @@ #include "watchdog.h" #include -// #include + #if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION) # error watchdog support requires CONFIG_SCHED_INSTRUMENTATION diff --git a/apps/storage/replay/Replay.cpp b/apps/storage/replay/Replay.cpp index 7a644da3b4..956180c0f4 100644 --- a/apps/storage/replay/Replay.cpp +++ b/apps/storage/replay/Replay.cpp @@ -17,13 +17,12 @@ * @author Beat Kueng */ -#include -#include -#include -#include -#include -#include -#include +#include +#include + + +#include +#include #include #include @@ -313,13 +312,13 @@ string Replay::parseOrbFields(const string &fields) { const char *c_type = orb_get_c_type(fields[format_idx]); if (c_type) { - string str_type = c_type; - ret += str_type; + string str_type = c_type; + ret += str_type; ++format_idx; } - int len = end_field - (fields.c_str() + format_idx) + 1; - ret += fields.substr(format_idx, len); + int len = end_field - (fields.c_str() + format_idx) + 1; + ret += fields.substr(format_idx, len); format_idx += len; } @@ -366,20 +365,17 @@ bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) { if (topic_name == "sensor_combined") { if (orb_fields == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;" "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" - "uint32_t accelerometer_integral_dt" && - file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;" - "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" - "float accelerometer_integral_dt;") { + "uint32_t accelerometer_integral_dt" + && file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;" + "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;" + "float accelerometer_integral_dt;") { int gyro_integral_dt_offset_log; int gyro_integral_dt_offset_intern; int accelerometer_integral_dt_offset_log; int accelerometer_integral_dt_offset_intern; int unused; - if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) && - findFieldOffset(orb_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && - findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) && - findFieldOffset(orb_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { + if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) && findFieldOffset(orb_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) && findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) && findFieldOffset(orb_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) { compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern, accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern); } @@ -963,8 +959,7 @@ bool Replay::publishTopic(Subscription &sub, void *data) { } if (subscription->orb_meta) { - if (strcmp(sub.orb_meta->o_name, subscription->orb_meta->o_name) == 0 && - subscription->orb_advert && subscription->multi_id == sub.multi_id - 1) { + if (strcmp(sub.orb_meta->o_name, subscription->orb_meta->o_name) == 0 && subscription->orb_advert && subscription->multi_id == sub.multi_id - 1) { advertised = true; } } diff --git a/apps/storage/replay/Replay.hpp b/apps/storage/replay/Replay.hpp index ab0fe37dd6..085a428021 100644 --- a/apps/storage/replay/Replay.hpp +++ b/apps/storage/replay/Replay.hpp @@ -18,7 +18,7 @@ #include "definitions.hpp" -#include +#include #include #include @@ -106,7 +106,7 @@ class Replay : public ModuleBase { uint8_t multi_id; int timestamp_offset; ///< marks the field of the timestamp - bool ignored = false; ///< if true, it will not be considered for publication in the main loop + bool ignored = false; ///< if true, it will not be considered for publication in the main loop std::streampos next_read_pos; uint64_t next_timestamp; ///< timestamp of the file diff --git a/apps/storage/replay/ReplayEkf2.cpp b/apps/storage/replay/ReplayEkf2.cpp index bd26211138..f17d818aaf 100644 --- a/apps/storage/replay/ReplayEkf2.cpp +++ b/apps/storage/replay/ReplayEkf2.cpp @@ -8,11 +8,11 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include -#include -#include +#include +#include -#include + +#include // for ekf2 replay #include diff --git a/apps/telemetry/mavlink/mavlink.c b/apps/telemetry/mavlink/mavlink.c index 3dbb4e67b6..b98e6cbabd 100644 --- a/apps/telemetry/mavlink/mavlink.c +++ b/apps/telemetry/mavlink/mavlink.c @@ -15,7 +15,7 @@ * @author Lorenz Meier */ -#include + #include #include #include diff --git a/apps/telemetry/mavlink/mavlink_command_sender.cpp b/apps/telemetry/mavlink/mavlink_command_sender.cpp index 999a590216..c82bb926e4 100644 --- a/apps/telemetry/mavlink/mavlink_command_sender.cpp +++ b/apps/telemetry/mavlink/mavlink_command_sender.cpp @@ -16,7 +16,7 @@ */ #include "mavlink_command_sender.h" -#include +#include #define CMD_DEBUG(FMT, ...) PX4_LOG_NAMED_COND("cmd sender", _debug_enabled, FMT, ##__VA_ARGS__) diff --git a/apps/telemetry/mavlink/mavlink_command_sender.h b/apps/telemetry/mavlink/mavlink_command_sender.h index 00b1ea0634..3ed20f1189 100644 --- a/apps/telemetry/mavlink/mavlink_command_sender.h +++ b/apps/telemetry/mavlink/mavlink_command_sender.h @@ -17,9 +17,8 @@ #pragma once -#include -#include -#include + +#include #include #include diff --git a/apps/telemetry/mavlink/mavlink_events.h b/apps/telemetry/mavlink/mavlink_events.h index fad2ee4a58..5c7a95234c 100644 --- a/apps/telemetry/mavlink/mavlink_events.h +++ b/apps/telemetry/mavlink/mavlink_events.h @@ -13,9 +13,9 @@ #include #include "mavlink_bridge_header.h" -#include -#include -#include +#include +#include +#include #include using namespace time_literals; diff --git a/apps/telemetry/mavlink/mavlink_ftp.h b/apps/telemetry/mavlink/mavlink_ftp.h index 7f8035cb2f..56feff702a 100644 --- a/apps/telemetry/mavlink/mavlink_ftp.h +++ b/apps/telemetry/mavlink/mavlink_ftp.h @@ -16,9 +16,9 @@ #include #include -#include +#include #include -#include +#include #ifndef MAVLINK_FTP_UNIT_TEST # include "mavlink_bridge_header.h" diff --git a/apps/telemetry/mavlink/mavlink_log_handler.h b/apps/telemetry/mavlink/mavlink_log_handler.h index 9bac6c59b4..eb56050441 100644 --- a/apps/telemetry/mavlink/mavlink_log_handler.h +++ b/apps/telemetry/mavlink/mavlink_log_handler.h @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include "mavlink_bridge_header.h" diff --git a/apps/telemetry/mavlink/mavlink_main.cpp b/apps/telemetry/mavlink/mavlink_main.cpp index 90500a1732..0944515439 100644 --- a/apps/telemetry/mavlink/mavlink_main.cpp +++ b/apps/telemetry/mavlink/mavlink_main.cpp @@ -26,13 +26,11 @@ #endif #include -#include -#include -#include -#include - -#include - +#include +#include +#include +#include +#include #include #include "mavlink_receiver.h" #include "mavlink_main.h" diff --git a/apps/telemetry/mavlink/mavlink_main.h b/apps/telemetry/mavlink/mavlink_main.h index aeb7152800..7ee4e46efb 100644 --- a/apps/telemetry/mavlink/mavlink_main.h +++ b/apps/telemetry/mavlink/mavlink_main.h @@ -37,15 +37,15 @@ #include #include -#include +#include #include #include -#include -#include -#include -#include -#include -#include + +#include +#include +#include +#include + #include #include #include diff --git a/apps/telemetry/mavlink/mavlink_messages.cpp b/apps/telemetry/mavlink/mavlink_messages.cpp index e05d73a1b2..73ff6f6859 100644 --- a/apps/telemetry/mavlink/mavlink_messages.cpp +++ b/apps/telemetry/mavlink/mavlink_messages.cpp @@ -22,11 +22,11 @@ #include "mavlink_simple_analyzer.h" #include -#include -#include -#include -#include -#include +#include +#include +#include +#include + #include #include diff --git a/apps/telemetry/mavlink/mavlink_mission.cpp b/apps/telemetry/mavlink/mavlink_mission.cpp index eb7e9588a0..25674a56a0 100644 --- a/apps/telemetry/mavlink/mavlink_mission.cpp +++ b/apps/telemetry/mavlink/mavlink_mission.cpp @@ -20,11 +20,10 @@ #include "mavlink_mission.h" #include "mavlink_main.h" -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -42,10 +41,7 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; -#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ - ((_msg.target_component == mavlink_system.compid) || \ - (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ - (_msg.target_component == MAV_COMP_ID_ALL))) +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && ((_msg.target_component == mavlink_system.compid) || (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || (_msg.target_component == MAV_COMP_ID_ALL))) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink) { @@ -271,8 +267,7 @@ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uin mission_item.lon = mission_fence_point.lon; mission_item.altitude = mission_fence_point.alt; - if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || - mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { mission_item.vertex_count = mission_fence_point.vertex_count; } else { @@ -857,8 +852,7 @@ void MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) { _transfer_partner_sysid = msg->sysid; _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; - _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission + _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission _transfer_current_seq = -1; if (_mission_type == MAV_MISSION_TYPE_FENCE) { @@ -1014,11 +1008,7 @@ void MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *ms case MAV_MISSION_TYPE_MISSION: { // check that we don't get a wrong item (hardening against wrong client implementations, the list here // does not need to be complete) - if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { check_failed = true; } else { @@ -1042,8 +1032,7 @@ void MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *ms mission_fence_point.lon = mission_item.lon; mission_fence_point.alt = mission_item.altitude; - if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { mission_fence_point.vertex_count = mission_item.vertex_count; if (mission_item.vertex_count < 3) { // feasibility check @@ -1060,7 +1049,8 @@ void MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *ms if (!check_failed) { write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + sizeof(mission_fence_point_s)) + != sizeof(mission_fence_point_s); } } break; @@ -1072,7 +1062,8 @@ void MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *ms mission_safe_point.alt = mission_item.altitude; mission_safe_point.frame = mission_item.frame; write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point, - sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); + sizeof(mission_safe_point_s)) + != sizeof(mission_safe_point_s); } break; default: @@ -1165,8 +1156,7 @@ void MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *ms switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, + ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; @@ -1179,8 +1169,7 @@ void MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *ms break; case MAV_MISSION_TYPE_ALL: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, + ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); ret = update_geofence_count(0) || ret; ret = update_safepoint_count(0) || ret; @@ -1212,15 +1201,11 @@ void MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *ms int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { - if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || - (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { // This is a mission item with a global coordinate // Switch to int mode if that is what we are receiving - if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { + if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { _int_mode = true; } @@ -1239,12 +1224,10 @@ int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item mission_item->altitude = mavlink_mission_item->z; - if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT) { + if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT) { mission_item->altitude_is_relative = false; - } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { mission_item->altitude_is_relative = true; } diff --git a/apps/telemetry/mavlink/mavlink_parameters.cpp b/apps/telemetry/mavlink/mavlink_parameters.cpp index bf29254c33..71734c3b4a 100644 --- a/apps/telemetry/mavlink/mavlink_parameters.cpp +++ b/apps/telemetry/mavlink/mavlink_parameters.cpp @@ -21,7 +21,7 @@ #include "mavlink_parameters.h" #include "mavlink_main.h" -#include +#include MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : _mavlink(mavlink) { @@ -39,8 +39,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); - if (req_list.target_system == mavlink_system.sysid && - (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { if (_send_all_index < 0) { _send_all_index = PARAM_HASH; @@ -52,8 +51,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS) - if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && - (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { // publish list request to UAVCAN driver via uORB. uavcan_parameter_request_s req{}; req.message_type = msg->msgid; @@ -72,8 +70,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); - if (set.target_system == mavlink_system.sysid && - (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -96,8 +93,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { if (param == PARAM_INVALID) { PX4_ERR("unknown param: %s", name); - } else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) || - (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) { + } else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) || (param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) { PX4_ERR("param types mismatch param: %s", name); } else { @@ -109,8 +105,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS) - if (set.target_system == mavlink_system.sysid && set.target_component < 127 && - (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. uavcan_parameter_request_s req{}; req.message_type = msg->msgid; @@ -143,8 +138,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); - if (req_read.target_system == mavlink_system.sysid && - (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* XXX: I left this in so older versions of QGC wouldn't break */ @@ -186,8 +180,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS) - if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && - (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. uavcan_parameter_request_s req{}; req.timestamp = hrt_absolute_time(); @@ -211,9 +204,7 @@ void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { mavlink_param_map_rc_t map_rc; mavlink_msg_param_map_rc_decode(msg, &map_rc); - if (map_rc.target_system == mavlink_system.sysid && - (map_rc.target_component == mavlink_system.compid || - map_rc.target_component == MAV_COMP_ID_ALL)) { + if (map_rc.target_system == mavlink_system.sysid && (map_rc.target_component == mavlink_system.compid || map_rc.target_component == MAV_COMP_ID_ALL)) { /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; @@ -297,7 +288,8 @@ void MavlinkParametersManager::send() { int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() && send_params()) {} + while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() && send_params()) { + } } bool MavlinkParametersManager::send_params() { @@ -457,8 +449,8 @@ int MavlinkParametersManager::send_param(param_t param, int component_id) { msg.param_index = param_get_used_index(param); #if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wstringop-truncation" +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wstringop-truncation" #endif /* * coverity[buffer_size_warning : FALSE] @@ -469,7 +461,7 @@ int MavlinkParametersManager::send_param(param_t param, int component_id) { */ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); #if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic pop +# pragma GCC diagnostic pop #endif /* query parameter type */ @@ -519,9 +511,9 @@ bool MavlinkParametersManager::send_uavcan() { mavlink_param_value_t msg{}; msg.param_count = value.param_count; msg.param_index = value.param_index; -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic ignored "-Wstringop-truncation" -#endif +# if defined(__GNUC__) && __GNUC__ >= 8 +# pragma GCC diagnostic ignored "-Wstringop-truncation" +# endif /* * coverity[buffer_size_warning : FALSE] * @@ -530,9 +522,9 @@ bool MavlinkParametersManager::send_uavcan() { * when copying it. */ strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); -#if defined(__GNUC__) && __GNUC__ >= 8 -#pragma GCC diagnostic pop -#endif +# if defined(__GNUC__) && __GNUC__ >= 8 +# pragma GCC diagnostic pop +# endif if (value.param_type == MAV_PARAM_TYPE_REAL32) { msg.param_type = MAVLINK_TYPE_FLOAT; diff --git a/apps/telemetry/mavlink/mavlink_parameters.h b/apps/telemetry/mavlink/mavlink_parameters.h index 52685f68a9..390baaac25 100644 --- a/apps/telemetry/mavlink/mavlink_parameters.h +++ b/apps/telemetry/mavlink/mavlink_parameters.h @@ -24,10 +24,9 @@ #include "mavlink_bridge_header.h" #include #include -#include #include #include -#include +#include #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS) # include diff --git a/apps/telemetry/mavlink/mavlink_rate_limiter.h b/apps/telemetry/mavlink/mavlink_rate_limiter.h index f2d96e308c..c83ff401ca 100644 --- a/apps/telemetry/mavlink/mavlink_rate_limiter.h +++ b/apps/telemetry/mavlink/mavlink_rate_limiter.h @@ -18,7 +18,7 @@ #ifndef MAVLINK_RATE_LIMITER_H_ #define MAVLINK_RATE_LIMITER_H_ -#include +#include class MavlinkRateLimiter { private: diff --git a/apps/telemetry/mavlink/mavlink_receiver.cpp b/apps/telemetry/mavlink/mavlink_receiver.cpp index 9fc4b2bbfe..cfc602a1ee 100644 --- a/apps/telemetry/mavlink/mavlink_receiver.cpp +++ b/apps/telemetry/mavlink/mavlink_receiver.cpp @@ -17,9 +17,9 @@ * @author Thomas Gubler */ -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ #include "mavlink_main.h" #include "mavlink_receiver.h" -#include // For DeviceId union +#include // For DeviceId union #ifdef CONFIG_NET # define MAVLINK_RECEIVER_NET_ADDED_STACK 1360 diff --git a/apps/telemetry/mavlink/mavlink_receiver.h b/apps/telemetry/mavlink/mavlink_receiver.h index fe6d6f43c9..59d8e435ee 100644 --- a/apps/telemetry/mavlink/mavlink_receiver.h +++ b/apps/telemetry/mavlink/mavlink_receiver.h @@ -29,11 +29,11 @@ #include "tune_publisher.h" #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/apps/telemetry/mavlink/mavlink_shell.cpp b/apps/telemetry/mavlink/mavlink_shell.cpp index b181406f55..c604a804f5 100644 --- a/apps/telemetry/mavlink/mavlink_shell.cpp +++ b/apps/telemetry/mavlink/mavlink_shell.cpp @@ -16,24 +16,24 @@ */ #include "mavlink_shell.h" -#include -#include -#include +#include +#include + #include #include #include #ifdef __PX4_NUTTX -#include +# include #endif /* __PX4_NUTTX */ #ifdef __PX4_POSIX -#include "../../../platforms/posix/src/px4/common/px4_daemon/pxh.h" +# include "../../../platforms/posix/src/px4/common/px4_daemon/pxh.h" #endif /* __PX4_POSIX */ #ifdef __PX4_CYGWIN -#include +# include #endif MavlinkShell::~MavlinkShell() { diff --git a/apps/telemetry/mavlink/mavlink_shell.h b/apps/telemetry/mavlink/mavlink_shell.h index 29f4ed077b..38d59f9c9d 100644 --- a/apps/telemetry/mavlink/mavlink_shell.h +++ b/apps/telemetry/mavlink/mavlink_shell.h @@ -17,8 +17,7 @@ #include #include -#include -#include +#include #pragma once @@ -61,6 +60,7 @@ class MavlinkShell { uint8_t targetSysid() const { return _target_sysid.load(); } + uint8_t targetCompid() const { return _target_compid.load(); } diff --git a/apps/telemetry/mavlink/mavlink_simple_analyzer.cpp b/apps/telemetry/mavlink/mavlink_simple_analyzer.cpp index 533694f402..2a618f2373 100644 --- a/apps/telemetry/mavlink/mavlink_simple_analyzer.cpp +++ b/apps/telemetry/mavlink/mavlink_simple_analyzer.cpp @@ -18,8 +18,8 @@ #include -#include -#include +#include +#include SimpleAnalyzer::SimpleAnalyzer(Mode mode, float window) : _window(window), diff --git a/apps/telemetry/mavlink/mavlink_stream.h b/apps/telemetry/mavlink/mavlink_stream.h index d5e8809dc6..f2d524fef2 100644 --- a/apps/telemetry/mavlink/mavlink_stream.h +++ b/apps/telemetry/mavlink/mavlink_stream.h @@ -18,8 +18,8 @@ #ifndef MAVLINK_STREAM_H_ #define MAVLINK_STREAM_H_ -#include -#include +#include +#include #include class Mavlink; diff --git a/apps/telemetry/mavlink/mavlink_timesync.h b/apps/telemetry/mavlink/mavlink_timesync.h index 6b455b8527..525e2ae7a6 100644 --- a/apps/telemetry/mavlink/mavlink_timesync.h +++ b/apps/telemetry/mavlink/mavlink_timesync.h @@ -19,7 +19,7 @@ #include "mavlink_bridge_header.h" -#include +#include class Mavlink; diff --git a/apps/telemetry/mavlink/mavlink_ulog.cpp b/apps/telemetry/mavlink/mavlink_ulog.cpp index a0b2667653..35767a8e38 100644 --- a/apps/telemetry/mavlink/mavlink_ulog.cpp +++ b/apps/telemetry/mavlink/mavlink_ulog.cpp @@ -16,7 +16,7 @@ */ #include "mavlink_ulog.h" -#include +#include #include #include diff --git a/apps/telemetry/mavlink/mavlink_ulog.h b/apps/telemetry/mavlink/mavlink_ulog.h index 0c4cd1064c..9847b6e069 100644 --- a/apps/telemetry/mavlink/mavlink_ulog.h +++ b/apps/telemetry/mavlink/mavlink_ulog.h @@ -19,10 +19,9 @@ #include #include -#include -#include -#include -#include + +#include +#include #include #include diff --git a/apps/telemetry/mavlink/streams/COMPONENT_INFORMATION.hpp b/apps/telemetry/mavlink/streams/COMPONENT_INFORMATION.hpp index bd307a4392..a5f773cff6 100644 --- a/apps/telemetry/mavlink/streams/COMPONENT_INFORMATION.hpp +++ b/apps/telemetry/mavlink/streams/COMPONENT_INFORMATION.hpp @@ -12,11 +12,8 @@ #define COMPONENT_INFORMATION_HPP #include "../mavlink_stream.h" - #include - -#include - +#include #include class MavlinkStreamComponentInformation : public MavlinkStream { @@ -28,6 +25,7 @@ class MavlinkStreamComponentInformation : public MavlinkStream { static constexpr const char *get_name_static() { return "COMPONENT_INFORMATION"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COMPONENT_INFORMATION; } @@ -35,6 +33,7 @@ class MavlinkStreamComponentInformation : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/COMPONENT_METADATA.hpp b/apps/telemetry/mavlink/streams/COMPONENT_METADATA.hpp index 5005e3373b..497dce870e 100644 --- a/apps/telemetry/mavlink/streams/COMPONENT_METADATA.hpp +++ b/apps/telemetry/mavlink/streams/COMPONENT_METADATA.hpp @@ -15,7 +15,7 @@ #include -#include +#include #include @@ -28,6 +28,7 @@ class MavlinkStreamComponentMetadata : public MavlinkStream { static constexpr const char *get_name_static() { return "COMPONENT_METADATA"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COMPONENT_METADATA; } @@ -35,6 +36,7 @@ class MavlinkStreamComponentMetadata : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/HIGH_LATENCY2.hpp b/apps/telemetry/mavlink/streams/HIGH_LATENCY2.hpp index 46dacb0240..ebf4ece9b2 100644 --- a/apps/telemetry/mavlink/streams/HIGH_LATENCY2.hpp +++ b/apps/telemetry/mavlink/streams/HIGH_LATENCY2.hpp @@ -11,9 +11,9 @@ #ifndef HIGH_LATENCY2_HPP #define HIGH_LATENCY2_HPP -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -#include +#include class MavlinkStreamHighLatency2 : public MavlinkStream { public: @@ -45,6 +45,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { static constexpr const char *get_name_static() { return "HIGH_LATENCY2"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_HIGH_LATENCY2; } @@ -52,6 +53,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } @@ -83,6 +85,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { PerBatteryData(uint8_t instance) : subscription(ORB_ID(battery_status), instance) { } + uORB::Subscription subscription; SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE}; uint64_t timestamp{0}; @@ -98,8 +101,8 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { mavlink_high_latency2_t msg{}; set_default_values(msg); - bool updated = _airspeed.valid(); - updated |= _airspeed_sp.valid(); + bool updated = _airspeed.valid(); + updated |= _airspeed_sp.valid(); for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { updated |= _batteries[i].analyzer.valid(); @@ -144,7 +147,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { for (int i = 1; i < battery_status_s::MAX_INSTANCES; i++) { const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( - 100.0f); + 100.0f); if (battery_connected && battery_is_lowest) { lowest = i; @@ -283,9 +286,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { estimator_status_s estimator_status; if (_estimator_status_sub.update(&estimator_status)) { - if (estimator_status.gps_check_fail_flags > 0 || - estimator_status.filter_fault_flags > 0 || - estimator_status.innovation_check_flags > 0) { + if (estimator_status.gps_check_fail_flags > 0 || estimator_status.filter_fault_flags > 0 || estimator_status.innovation_check_flags > 0) { msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; } @@ -385,23 +386,19 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { health_report_s health_report; if (_health_report_sub.copy(&health_report)) { - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::absolute_pressure) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t)events::px4::enums::health_component_t::absolute_pressure) { msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; } - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::accel) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t)events::px4::enums::health_component_t::accel) { msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; } - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::gyro) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t)events::px4::enums::health_component_t::gyro) { msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; } - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::magnetometer) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t)events::px4::enums::health_component_t::magnetometer) { msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; } } @@ -414,6 +411,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { union px4_custom_mode custom_mode { get_px4_custom_mode(status.nav_state) }; + msg->custom_mode = custom_mode.custom_mode_hl; return true; @@ -449,8 +447,8 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { if (_wind_sub.update(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, - wind.windspeed_north))) * - 0.5f); + wind.windspeed_north))) + * 0.5f); return true; } diff --git a/apps/telemetry/mavlink/streams/MAG_CAL_REPORT.hpp b/apps/telemetry/mavlink/streams/MAG_CAL_REPORT.hpp index 8c562a3c2a..f8ccdd7746 100644 --- a/apps/telemetry/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/apps/telemetry/mavlink/streams/MAG_CAL_REPORT.hpp @@ -11,8 +11,7 @@ #ifndef MAG_CAL_REPORT_HPP #define MAG_CAL_REPORT_HPP -#include - +#include #include #include @@ -25,6 +24,7 @@ class MavlinkStreamMagCalReport : public MavlinkStream { static constexpr const char *get_name_static() { return "MAG_CAL_REPORT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MAG_CAL_REPORT; } @@ -32,6 +32,7 @@ class MavlinkStreamMagCalReport : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/SCALED_IMU.hpp b/apps/telemetry/mavlink/streams/SCALED_IMU.hpp index 9ff0b82c8e..f1d22c99d4 100644 --- a/apps/telemetry/mavlink/streams/SCALED_IMU.hpp +++ b/apps/telemetry/mavlink/streams/SCALED_IMU.hpp @@ -11,8 +11,8 @@ #ifndef SCALED_IMU_HPP #define SCALED_IMU_HPP -#include -#include +#include +#include #include #include @@ -27,6 +27,7 @@ class MavlinkStreamScaledIMU : public MavlinkStream { static constexpr const char *get_name_static() { return "SCALED_IMU"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU; } @@ -34,6 +35,7 @@ class MavlinkStreamScaledIMU : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/SCALED_IMU2.hpp b/apps/telemetry/mavlink/streams/SCALED_IMU2.hpp index 5287dc7040..08e83a9daf 100644 --- a/apps/telemetry/mavlink/streams/SCALED_IMU2.hpp +++ b/apps/telemetry/mavlink/streams/SCALED_IMU2.hpp @@ -11,8 +11,8 @@ #ifndef SCALED_IMU2_HPP #define SCALED_IMU2_HPP -#include -#include +#include +#include #include #include @@ -27,6 +27,7 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream { static constexpr const char *get_name_static() { return "SCALED_IMU2"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU2; } @@ -34,6 +35,7 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/SCALED_IMU3.hpp b/apps/telemetry/mavlink/streams/SCALED_IMU3.hpp index e6d43accdf..728a8985da 100644 --- a/apps/telemetry/mavlink/streams/SCALED_IMU3.hpp +++ b/apps/telemetry/mavlink/streams/SCALED_IMU3.hpp @@ -11,9 +11,8 @@ #ifndef SCALED_IMU3_HPP #define SCALED_IMU3_HPP -#include -#include - +#include +#include #include #include #include @@ -27,6 +26,7 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream { static constexpr const char *get_name_static() { return "SCALED_IMU3"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SCALED_IMU3; } @@ -34,6 +34,7 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/STATUSTEXT.hpp b/apps/telemetry/mavlink/streams/STATUSTEXT.hpp index b95e628fcc..e5da8f1842 100644 --- a/apps/telemetry/mavlink/streams/STATUSTEXT.hpp +++ b/apps/telemetry/mavlink/streams/STATUSTEXT.hpp @@ -13,7 +13,7 @@ #include -#include +#include class MavlinkStreamStatustext : public MavlinkStream { public: @@ -24,6 +24,7 @@ class MavlinkStreamStatustext : public MavlinkStream { static constexpr const char *get_name_static() { return "STATUSTEXT"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_STATUSTEXT; } @@ -31,6 +32,7 @@ class MavlinkStreamStatustext : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/streams/SYS_STATUS.hpp b/apps/telemetry/mavlink/streams/SYS_STATUS.hpp index 4d968cc47f..64619f0b2a 100644 --- a/apps/telemetry/mavlink/streams/SYS_STATUS.hpp +++ b/apps/telemetry/mavlink/streams/SYS_STATUS.hpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include class MavlinkStreamSysStatus : public MavlinkStream { public: @@ -26,6 +26,7 @@ class MavlinkStreamSysStatus : public MavlinkStream { static constexpr const char *get_name_static() { return "SYS_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SYS_STATUS; } @@ -33,6 +34,7 @@ class MavlinkStreamSysStatus : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } @@ -59,9 +61,7 @@ class MavlinkStreamSysStatus : public MavlinkStream { msg.onboard_control_sensors_present |= mav_sensor; } - if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | - health_report.health_error_flags | health_report.health_warning_flags) & - (uint64_t)health_component) == 0) { + if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | health_report.health_error_flags | health_report.health_warning_flags) & (uint64_t)health_component) == 0) { msg.onboard_control_sensors_health |= mav_sensor; } @@ -77,9 +77,7 @@ class MavlinkStreamSysStatus : public MavlinkStream { msg.onboard_control_sensors_present_extended |= mav_sensor; } - if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | - health_report.health_error_flags | health_report.health_warning_flags) & - (uint64_t)health_component) == 0) { + if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | health_report.health_error_flags | health_report.health_warning_flags) & (uint64_t)health_component) == 0) { msg.onboard_control_sensors_health_extended |= mav_sensor; } @@ -113,8 +111,7 @@ class MavlinkStreamSysStatus : public MavlinkStream { // When the last cached battery is not connected or the current battery level is lower than the cached battery level, // the current battery status is replaced with the cached value for (int i = 0; i < _battery_status_subs.size(); i++) { - if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) || (battery_status[i].remaining < - battery_status[lowest_battery_index].remaining))) { + if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) || (battery_status[i].remaining < battery_status[lowest_battery_index].remaining))) { lowest_battery_index = i; } } diff --git a/apps/telemetry/mavlink/streams/TIMESYNC.hpp b/apps/telemetry/mavlink/streams/TIMESYNC.hpp index 765d42dc3d..5944d5b0c8 100644 --- a/apps/telemetry/mavlink/streams/TIMESYNC.hpp +++ b/apps/telemetry/mavlink/streams/TIMESYNC.hpp @@ -11,7 +11,7 @@ #ifndef TIMESYNC_HPP #define TIMESYNC_HPP -#include +#include class MavlinkStreamTimesync : public MavlinkStream { public: @@ -22,6 +22,7 @@ class MavlinkStreamTimesync : public MavlinkStream { static constexpr const char *get_name_static() { return "TIMESYNC"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TIMESYNC; } @@ -29,6 +30,7 @@ class MavlinkStreamTimesync : public MavlinkStream { const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } diff --git a/apps/telemetry/mavlink/timestamped_list.h b/apps/telemetry/mavlink/timestamped_list.h index 16aeb4d2b0..7df815dab4 100644 --- a/apps/telemetry/mavlink/timestamped_list.h +++ b/apps/telemetry/mavlink/timestamped_list.h @@ -21,7 +21,7 @@ #pragma once -#include +#include /** * @class TimestampedList diff --git a/apps/telemetry/mavlink/tune_publisher.cpp b/apps/telemetry/mavlink/tune_publisher.cpp index fe0d8752c7..c686903b73 100644 --- a/apps/telemetry/mavlink/tune_publisher.cpp +++ b/apps/telemetry/mavlink/tune_publisher.cpp @@ -10,7 +10,7 @@ #include "tune_publisher.h" #include "string.h" -#include +#include void TunePublisher::set_tune_string(const char *tune, const hrt_abstime &now) { // The tune string needs to be 0 terminated. diff --git a/apps/telemetry/mavlink/tune_publisher.h b/apps/telemetry/mavlink/tune_publisher.h index 78d838eadb..f264170dc2 100644 --- a/apps/telemetry/mavlink/tune_publisher.h +++ b/apps/telemetry/mavlink/tune_publisher.h @@ -11,8 +11,8 @@ #pragma once #include -#include -#include +#include +#include class TunePublisher { public: diff --git a/apps/telemetry/uavcan/actuators/esc.cpp b/apps/telemetry/uavcan/actuators/esc.cpp index 35ca659402..710be52dc4 100644 --- a/apps/telemetry/uavcan/actuators/esc.cpp +++ b/apps/telemetry/uavcan/actuators/esc.cpp @@ -16,7 +16,7 @@ #include "esc.hpp" #include -#include +#include #define MOTOR_BIT(x) (1 << (x)) @@ -110,12 +110,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_rpm = msg.rpm; ref.esc_errorcount = msg.error_count; - _esc_status.esc_count = _rotor_count; - _esc_status.counter += 1; - _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; - _esc_status.esc_online_flags = check_escs_status(); - _esc_status.esc_armed_flags = (1 << _rotor_count) - 1; - _esc_status.timestamp = hrt_absolute_time(); + _esc_status.esc_count = _rotor_count; + _esc_status.counter += 1; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_online_flags = check_escs_status(); + _esc_status.esc_armed_flags = (1 << _rotor_count) - 1; + _esc_status.timestamp = hrt_absolute_time(); _esc_status_pub.publish(_esc_status); } } diff --git a/apps/telemetry/uavcan/actuators/esc.hpp b/apps/telemetry/uavcan/actuators/esc.hpp index 6eb1e30e60..b02557ff6f 100644 --- a/apps/telemetry/uavcan/actuators/esc.hpp +++ b/apps/telemetry/uavcan/actuators/esc.hpp @@ -24,12 +24,12 @@ #include #include #include -#include +#include #include #include #include -#include -#include +#include +#include class UavcanEscController { public: diff --git a/apps/telemetry/uavcan/actuators/servo.cpp b/apps/telemetry/uavcan/actuators/servo.cpp index ce2e96eae4..e238d89a0a 100644 --- a/apps/telemetry/uavcan/actuators/servo.cpp +++ b/apps/telemetry/uavcan/actuators/servo.cpp @@ -10,7 +10,7 @@ #include "servo.hpp" #include -#include +#include using namespace time_literals; diff --git a/apps/telemetry/uavcan/actuators/servo.hpp b/apps/telemetry/uavcan/actuators/servo.hpp index b3c6248a0d..d23ae7ca6e 100644 --- a/apps/telemetry/uavcan/actuators/servo.hpp +++ b/apps/telemetry/uavcan/actuators/servo.hpp @@ -13,11 +13,11 @@ #include #include #include -#include +#include #include #include -#include -#include +#include +#include class UavcanServoController { public: diff --git a/apps/telemetry/uavcan/beep.cpp b/apps/telemetry/uavcan/beep.cpp index 3e80dadf6a..1beb841558 100644 --- a/apps/telemetry/uavcan/beep.cpp +++ b/apps/telemetry/uavcan/beep.cpp @@ -16,7 +16,7 @@ #include "beep.hpp" -#include +#include UavcanBeepController::UavcanBeepController(uavcan::INode &node) : _beep_pub(node), diff --git a/apps/telemetry/uavcan/beep.hpp b/apps/telemetry/uavcan/beep.hpp index 1d889eaa69..93ec7c8b62 100644 --- a/apps/telemetry/uavcan/beep.hpp +++ b/apps/telemetry/uavcan/beep.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include class UavcanBeepController { public: diff --git a/apps/telemetry/uavcan/logmessage.hpp b/apps/telemetry/uavcan/logmessage.hpp index 2fa2bcd39b..8486a0b728 100644 --- a/apps/telemetry/uavcan/logmessage.hpp +++ b/apps/telemetry/uavcan/logmessage.hpp @@ -10,7 +10,7 @@ #pragma once -#include +#include #include #include @@ -20,6 +20,7 @@ class UavcanLogMessage { UavcanLogMessage(uavcan::INode &node) : _sub_logmessage(node) { } + ~UavcanLogMessage() = default; int init() { diff --git a/apps/telemetry/uavcan/rgbled.hpp b/apps/telemetry/uavcan/rgbled.hpp index e17064850f..1d3ce4ef8e 100644 --- a/apps/telemetry/uavcan/rgbled.hpp +++ b/apps/telemetry/uavcan/rgbled.hpp @@ -15,8 +15,8 @@ #include #include -#include -#include +#include +#include class UavcanRGBController : public ModuleParams { public: diff --git a/apps/telemetry/uavcan/sensors/accel.cpp b/apps/telemetry/uavcan/sensors/accel.cpp index b31848a351..6c4c1db0d9 100644 --- a/apps/telemetry/uavcan/sensors/accel.cpp +++ b/apps/telemetry/uavcan/sensors/accel.cpp @@ -13,7 +13,7 @@ */ #include "accel.hpp" -#include +#include const char *const UavcanAccelBridge::NAME = "accel"; diff --git a/apps/telemetry/uavcan/sensors/airspeed.cpp b/apps/telemetry/uavcan/sensors/airspeed.cpp index ea6754cb72..0df95c2554 100644 --- a/apps/telemetry/uavcan/sensors/airspeed.cpp +++ b/apps/telemetry/uavcan/sensors/airspeed.cpp @@ -12,10 +12,10 @@ * @author RJ Gritter */ -#include +#include #include "airspeed.hpp" #include -#include // For CONSTANTS_* +#include // For CONSTANTS_* const char *const UavcanAirspeedBridge::NAME = "airspeed"; @@ -58,6 +58,7 @@ void UavcanAirspeedBridge::oat_sub_cb(const uavcan::ReceivedDataStructure &msg) { _last_tas_m_s = msg.true_airspeed; } + void UavcanAirspeedBridge::ias_sub_cb(const uavcan::ReceivedDataStructure &msg) { airspeed_s report{}; diff --git a/apps/telemetry/uavcan/sensors/baro.cpp b/apps/telemetry/uavcan/sensors/baro.cpp index 4097bf0255..be75c2cde7 100644 --- a/apps/telemetry/uavcan/sensors/baro.cpp +++ b/apps/telemetry/uavcan/sensors/baro.cpp @@ -12,13 +12,13 @@ * @author Pavel Kirienko */ -#include +#include #include "baro.hpp" #include #include #include -#include // For CONSTANTS_* +#include // For CONSTANTS_* const char *const UavcanBarometerBridge::NAME = "baro"; diff --git a/apps/telemetry/uavcan/sensors/battery.cpp b/apps/telemetry/uavcan/sensors/battery.cpp index 459191d2f2..05c02c308d 100644 --- a/apps/telemetry/uavcan/sensors/battery.cpp +++ b/apps/telemetry/uavcan/sensors/battery.cpp @@ -10,9 +10,9 @@ #include "battery.hpp" -#include +#include #include -#include +#include const char *const UavcanBatteryBridge::NAME = "battery"; @@ -141,17 +141,12 @@ void UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure _batt_update_mod[instance] = BatteryDataType::RawAux; - _battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh - - _battery_status[instance].remaining_capacity_wh) / - msg.nominal_voltage * - 1000; + _battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh - _battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage * 1000; _battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14); _battery_status[instance].cycle_count = msg.cycle_count; _battery_status[instance].over_discharge_count = msg.over_discharge_count; _battery_status[instance].nominal_voltage = msg.nominal_voltage; - _battery_status[instance].time_remaining_s = math::isZero(_battery_status[instance].current_a) ? 0 : - (_battery_status[instance].remaining_capacity_wh / - _battery_status[instance].nominal_voltage / _battery_status[instance].current_a * 3600); + _battery_status[instance].time_remaining_s = math::isZero(_battery_status[instance].current_a) ? 0 : (_battery_status[instance].remaining_capacity_wh / _battery_status[instance].nominal_voltage / _battery_status[instance].current_a * 3600); _battery_status[instance].is_powering_off = msg.is_powering_off; for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) { @@ -174,8 +169,8 @@ void UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a) if (_last_timestamp != 0) { const float dt = (timestamp - _last_timestamp) / 1e6; // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) - _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); - _discharged_mah += _discharged_mah_loop; + _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); + _discharged_mah += _discharged_mah_loop; } _last_timestamp = timestamp; @@ -205,7 +200,7 @@ void UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructuregetBatteryStatus(); _battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celsius _battery_status[instance].serial_number = msg.model_instance_id; - _battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery + _battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery publish(msg.getSrcNodeID().get(), &_battery_status[instance]); } diff --git a/apps/telemetry/uavcan/sensors/battery.hpp b/apps/telemetry/uavcan/sensors/battery.hpp index 2454c7858e..f8209a8ccc 100644 --- a/apps/telemetry/uavcan/sensors/battery.hpp +++ b/apps/telemetry/uavcan/sensors/battery.hpp @@ -19,8 +19,8 @@ #include #include #include -#include -#include +#include +#include using namespace time_literals; diff --git a/apps/telemetry/uavcan/sensors/differential_pressure.cpp b/apps/telemetry/uavcan/sensors/differential_pressure.cpp index 903cb6554d..7ac725e294 100644 --- a/apps/telemetry/uavcan/sensors/differential_pressure.cpp +++ b/apps/telemetry/uavcan/sensors/differential_pressure.cpp @@ -14,8 +14,8 @@ #include "differential_pressure.hpp" -#include -#include +#include +#include #include #include diff --git a/apps/telemetry/uavcan/sensors/flow.cpp b/apps/telemetry/uavcan/sensors/flow.cpp index 47d7b954a0..b1136ac2fa 100644 --- a/apps/telemetry/uavcan/sensors/flow.cpp +++ b/apps/telemetry/uavcan/sensors/flow.cpp @@ -10,7 +10,7 @@ #include "flow.hpp" -#include +#include const char *const UavcanFlowBridge::NAME = "flow"; diff --git a/apps/telemetry/uavcan/sensors/gnss.cpp b/apps/telemetry/uavcan/sensors/gnss.cpp index d2dd9b2abd..ea10881bc1 100644 --- a/apps/telemetry/uavcan/sensors/gnss.cpp +++ b/apps/telemetry/uavcan/sensors/gnss.cpp @@ -20,10 +20,10 @@ #include -#include +#include #include #include -#include +#include using namespace time_literals; @@ -335,10 +335,9 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure float vel_n_sq = vel_n * vel_n; float vel_e_sq = vel_e * vel_e; report.c_variance_rad = - (vel_e_sq * vel_cov[0] + - -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric - vel_n_sq * vel_cov[4]) / - ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + (vel_e_sq * vel_cov[0] + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric + vel_n_sq * vel_cov[4]) + / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); } else { report.s_variance_m_s = -1.0F; @@ -350,9 +349,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.vel_n_m_s = msg.ned_velocity[0]; report.vel_e_m_s = msg.ned_velocity[1]; report.vel_d_m_s = msg.ned_velocity[2]; - report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + - report.vel_e_m_s * report.vel_e_m_s + - report.vel_d_m_s * report.vel_d_m_s); + report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s); report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s); report.vel_ned_valid = true; diff --git a/apps/telemetry/uavcan/sensors/gnss.hpp b/apps/telemetry/uavcan/sensors/gnss.hpp index eb4b8b49e1..629d61f3eb 100644 --- a/apps/telemetry/uavcan/sensors/gnss.hpp +++ b/apps/telemetry/uavcan/sensors/gnss.hpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include "sensor_bridge.hpp" @@ -106,9 +106,9 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase { hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections - bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? + bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? - bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg + bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg bool _publish_rtcm_stream{false}; bool _publish_moving_baseline_data{false}; diff --git a/apps/telemetry/uavcan/sensors/gyro.cpp b/apps/telemetry/uavcan/sensors/gyro.cpp index 757ac0e78c..535304e76a 100644 --- a/apps/telemetry/uavcan/sensors/gyro.cpp +++ b/apps/telemetry/uavcan/sensors/gyro.cpp @@ -13,7 +13,7 @@ */ #include "gyro.hpp" -#include +#include const char *const UavcanGyroBridge::NAME = "gyro"; diff --git a/apps/telemetry/uavcan/sensors/hygrometer.cpp b/apps/telemetry/uavcan/sensors/hygrometer.cpp index 511e86e680..744bd1ab19 100644 --- a/apps/telemetry/uavcan/sensors/hygrometer.cpp +++ b/apps/telemetry/uavcan/sensors/hygrometer.cpp @@ -10,8 +10,8 @@ #include "hygrometer.hpp" -#include -#include +#include +#include const char *const UavcanHygrometerBridge::NAME = "hygrometer_sensor"; diff --git a/apps/telemetry/uavcan/sensors/mag.cpp b/apps/telemetry/uavcan/sensors/mag.cpp index fe724c61f7..d701181855 100644 --- a/apps/telemetry/uavcan/sensors/mag.cpp +++ b/apps/telemetry/uavcan/sensors/mag.cpp @@ -14,10 +14,10 @@ #include "mag.hpp" -#include +#include #include -#include +#include const char *const UavcanMagnetometerBridge::NAME = "mag"; diff --git a/apps/telemetry/uavcan/sensors/rangefinder.cpp b/apps/telemetry/uavcan/sensors/rangefinder.cpp index 8832d77781..417a22edfd 100644 --- a/apps/telemetry/uavcan/sensors/rangefinder.cpp +++ b/apps/telemetry/uavcan/sensors/rangefinder.cpp @@ -12,7 +12,7 @@ * @author RJ Gritter */ -#include +#include #include #include "rangefinder.hpp" #include diff --git a/apps/telemetry/uavcan/sensors/safety_button.cpp b/apps/telemetry/uavcan/sensors/safety_button.cpp index be52edf0d6..2c876665d7 100644 --- a/apps/telemetry/uavcan/sensors/safety_button.cpp +++ b/apps/telemetry/uavcan/sensors/safety_button.cpp @@ -8,7 +8,7 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include +#include #include #include "safety_button.hpp" #include diff --git a/apps/telemetry/uavcan/sensors/sensor_bridge.hpp b/apps/telemetry/uavcan/sensors/sensor_bridge.hpp index 85b8306ba8..e1a2adddc5 100644 --- a/apps/telemetry/uavcan/sensors/sensor_bridge.hpp +++ b/apps/telemetry/uavcan/sensors/sensor_bridge.hpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include /** @@ -106,7 +106,7 @@ class UavcanSensorBridgeBase : public IUavcanSensorBridge, public device::Device */ virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; - }; + } uavcan_bridge::Channel *get_channel_for_node(int node_id); diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp index 6b030da825..97230ea3ea 100644 --- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp +++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include namespace uavcan_socketcan { diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp index b3a8ce1836..d627d2b728 100644 --- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp +++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -53,7 +53,7 @@ #define MODULE_NAME "UAVCAN_SOCKETCAN" -#include +#include namespace uavcan_socketcan { diff --git a/apps/telemetry/uavcan/uavcan_main.cpp b/apps/telemetry/uavcan/uavcan_main.cpp index ba01d31e2f..15e0105e53 100644 --- a/apps/telemetry/uavcan/uavcan_main.cpp +++ b/apps/telemetry/uavcan/uavcan_main.cpp @@ -19,8 +19,6 @@ * */ -#include -#include #include #include @@ -34,7 +32,7 @@ #include -#include +#include #include "uavcan_module.hpp" #include "uavcan_main.hpp" @@ -390,8 +388,8 @@ void UavcanNode::fill_node_info() { // Extracting the first 8 hex digits of the git hash and converting them to int char fw_git_short[9] = {}; std::memmove(fw_git_short, px4_firmware_version_string(), 8); - char *end = nullptr; - swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; // Too verbose for normal operation @@ -1092,11 +1090,11 @@ void UavcanNode::param_opcode(uavcan::NodeID node_id) { } void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) { - bool success = result.isSuccessful(); - uint8_t node_id = result.getCallID().server_node_id.get(); - uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); - success &= resp.ok; - _cmd_in_progress = false; + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; if (!result.isSuccessful()) { PX4_ERR("save request for node %hhu timed out.", node_id); @@ -1136,11 +1134,11 @@ void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) { - bool success = result.isSuccessful(); - uint8_t node_id = result.getCallID().server_node_id.get(); - uavcan::protocol::RestartNode::Response resp = result.getResponse(); - success &= resp.ok; - _cmd_in_progress = false; + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::RestartNode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; if (success) { PX4_DEBUG("restart request for node %hhu completed OK.", node_id); diff --git a/apps/telemetry/uavcan/uavcan_main.hpp b/apps/telemetry/uavcan/uavcan_main.hpp index 6dd50552dc..112b84e65c 100644 --- a/apps/telemetry/uavcan/uavcan_main.hpp +++ b/apps/telemetry/uavcan/uavcan_main.hpp @@ -19,9 +19,10 @@ #pragma once -#include -#include -#include + +#include +#include + #include "actuators/esc.hpp" #include "actuators/hardpoint.hpp" @@ -36,9 +37,9 @@ #include "uavcan_driver.hpp" #include "uavcan_servers.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -154,11 +155,14 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams { public: typedef UAVCAN_DRIVER::CanInitHelper CanInitHelper; - enum eServerAction : int { None, - Start, - Stop, - CheckFW, - Busy }; + + enum eServerAction : int { + None, + Start, + Stop, + CheckFW, + Busy + }; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -177,6 +181,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams { static UavcanNode *instance() { return _instance; } + static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); void requestCheckAllNodesFirmwareAndUpdate() { @@ -204,15 +209,16 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams { void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; } + void free_setget_response(void) { _setget_response = nullptr; } px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver - unsigned _output_count{0}; ///< number of actuators currently available + unsigned _output_count{0}; ///< number of actuators currently available - static UavcanNode *_instance; ///< singleton pointer + static UavcanNode *_instance; ///< singleton pointer uavcan_node::Allocator _pool_allocator; @@ -309,12 +315,15 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams { uint8_t get_next_active_node_id(uint8_t base); uint8_t get_next_dirty_node_id(uint8_t base); - void set_node_params_dirty(uint8_t node_id) { + + void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); } + void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); } + bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); } diff --git a/apps/telemetry/uavcan/uavcan_module.hpp b/apps/telemetry/uavcan/uavcan_module.hpp index 19fc642fa8..0cdf2883c2 100644 --- a/apps/telemetry/uavcan/uavcan_module.hpp +++ b/apps/telemetry/uavcan/uavcan_module.hpp @@ -10,7 +10,6 @@ #pragma once -#include #include diff --git a/apps/telemetry/uavcan/uavcan_servers.cpp b/apps/telemetry/uavcan/uavcan_servers.cpp index b3693cda97..6697c8b4cc 100644 --- a/apps/telemetry/uavcan/uavcan_servers.cpp +++ b/apps/telemetry/uavcan/uavcan_servers.cpp @@ -8,8 +8,8 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include -#include + +#include #include @@ -234,7 +234,7 @@ int UavcanServers::copyFw(const char *dst, const char *src) { } else { total_written += written; - remaining -= written; + remaining -= written; } } while (written > 0 && remaining > 0); } diff --git a/apps/telemetry/uavcan/uavcan_servers.hpp b/apps/telemetry/uavcan/uavcan_servers.hpp index fbf1fddaef..56e06ce0e1 100644 --- a/apps/telemetry/uavcan/uavcan_servers.hpp +++ b/apps/telemetry/uavcan/uavcan_servers.hpp @@ -10,7 +10,6 @@ #pragma once -#include #include #include diff --git a/apps/telemetry/uxrce_dds_client/Micro-XRCE-DDS-Client/src/c/util/time.c b/apps/telemetry/uxrce_dds_client/Micro-XRCE-DDS-Client/src/c/util/time.c index 12aec56d0a..b8dfeb6cca 100644 --- a/apps/telemetry/uxrce_dds_client/Micro-XRCE-DDS-Client/src/c/util/time.c +++ b/apps/telemetry/uxrce_dds_client/Micro-XRCE-DDS-Client/src/c/util/time.c @@ -21,7 +21,7 @@ #endif #define __EXPORT __attribute__ ((visibility ("default"))) -#include +#include #endif // PX4 diff --git a/apps/telemetry/uxrce_dds_client/uxrce_dds_client.cpp b/apps/telemetry/uxrce_dds_client/uxrce_dds_client.cpp index e90de98762..d2274f03ec 100644 --- a/apps/telemetry/uxrce_dds_client/uxrce_dds_client.cpp +++ b/apps/telemetry/uxrce_dds_client/uxrce_dds_client.cpp @@ -8,9 +8,9 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include +#include #include -#include + #include "uxrce_dds_client.h" @@ -24,11 +24,11 @@ #include #if defined(CONFIG_NET) || defined(__PX4_POSIX) -#define UXRCE_DDS_CLIENT_UDP 1 +# define UXRCE_DDS_CLIENT_UDP 1 #endif -#define STREAM_HISTORY 4 -#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default +#define STREAM_HISTORY 4 +#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default #define PARTICIPANT_XML_SIZE 512 @@ -205,34 +205,30 @@ void UxrceddsClient::run() { char participant_xml[PARTICIPANT_XML_SIZE]; int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s%s/px4_micro_xrce_dds%s", - _localhost_only ? - "" - "" - "" - "" - "udp_localhost" - "UDPv4" - "
127.0.0.1
" - "" - "" - "" - "" - "" : - "" - "" - "", - _client_namespace != nullptr ? - _client_namespace : - "", - _localhost_only ? - "false" - "udp_localhost" - "" - "" - "" : - "" - "" - ""); + _localhost_only ? "" + "" + "" + "" + "udp_localhost" + "UDPv4" + "
127.0.0.1
" + "" + "" + "" + "" + "" + : "" + "" + "", + _client_namespace != nullptr ? _client_namespace : "", + _localhost_only ? "false" + "udp_localhost" + "" + "" + "" + : "" + "" + ""); if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) { PX4_ERR("create entities failed: namespace too long"); @@ -355,71 +351,101 @@ int UxrceddsClient::setBaudrate(int fd, unsigned baud) { int speed; switch (baud) { - case 9600: speed = B9600; break; + case 9600: + speed = B9600; + break; - case 19200: speed = B19200; break; + case 19200: + speed = B19200; + break; - case 38400: speed = B38400; break; + case 38400: + speed = B38400; + break; - case 57600: speed = B57600; break; + case 57600: + speed = B57600; + break; - case 115200: speed = B115200; break; + case 115200: + speed = B115200; + break; - case 230400: speed = B230400; break; + case 230400: + speed = B230400; + break; #ifndef B460800 -#define B460800 460800 +# define B460800 460800 #endif - case 460800: speed = B460800; break; + case 460800: + speed = B460800; + break; #ifndef B921600 -#define B921600 921600 +# define B921600 921600 #endif - case 921600: speed = B921600; break; + case 921600: + speed = B921600; + break; #ifndef B1000000 -#define B1000000 1000000 +# define B1000000 1000000 #endif - case 1000000: speed = B1000000; break; + case 1000000: + speed = B1000000; + break; #ifndef B1500000 -#define B1500000 1500000 +# define B1500000 1500000 #endif - case 1500000: speed = B1500000; break; + case 1500000: + speed = B1500000; + break; #ifndef B2000000 -#define B2000000 2000000 +# define B2000000 2000000 #endif - case 2000000: speed = B2000000; break; + case 2000000: + speed = B2000000; + break; #ifndef B2500000 -#define B2500000 2500000 +# define B2500000 2500000 #endif - case 2500000: speed = B2500000; break; + case 2500000: + speed = B2500000; + break; #ifndef B3000000 -#define B3000000 3000000 +# define B3000000 3000000 #endif - case 3000000: speed = B3000000; break; + case 3000000: + speed = B3000000; + break; #ifndef B3500000 -#define B3500000 3500000 +# define B3500000 3500000 #endif - case 3500000: speed = B3500000; break; + case 3500000: + speed = B3500000; + break; #ifndef B4000000 -#define B4000000 4000000 +# define B4000000 4000000 #endif - case 4000000: speed = B4000000; break; + case 4000000: + speed = B4000000; + break; default: PX4_ERR("ERR: unknown baudrate: %d", baud); @@ -443,8 +469,7 @@ int UxrceddsClient::setBaudrate(int fd, unsigned baud) { // no input parity check, don't strip high bit off, // no XON/XOFF software flow control // - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | - INLCR | PARMRK | INPCK | ISTRIP | IXON); + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); // // Output flags - Turn off output processing // diff --git a/apps/telemetry/uxrce_dds_client/uxrce_dds_client.h b/apps/telemetry/uxrce_dds_client/uxrce_dds_client.h index 4051615cdc..e0445dc766 100644 --- a/apps/telemetry/uxrce_dds_client/uxrce_dds_client.h +++ b/apps/telemetry/uxrce_dds_client/uxrce_dds_client.h @@ -10,12 +10,12 @@ #pragma once -#include -#include +#include +#include #include -#include +#include class UxrceddsClient : public ModuleBase, public ModuleParams { public: diff --git a/pkgs/board_identity/board_identity.c b/pkgs/board_identity/board_identity.c index 06e87c77f0..68b07cc478 100644 --- a/pkgs/board_identity/board_identity.c +++ b/pkgs/board_identity/board_identity.c @@ -13,7 +13,6 @@ * Implementation of Non Arch specific Board identity API */ -// #include #include #include diff --git a/pkgs/board_identity/board_identity_test.cpp b/pkgs/board_identity/board_identity_test.cpp index 64970f4394..fe8d3bd758 100644 --- a/pkgs/board_identity/board_identity_test.cpp +++ b/pkgs/board_identity/board_identity_test.cpp @@ -9,7 +9,6 @@ ******************************************************************/ #include -// #include #include diff --git a/pkgs/board_identity/stm32/board_identity.c b/pkgs/board_identity/stm32/board_identity.c index dd0a98f204..75bd3d8a0a 100644 --- a/pkgs/board_identity/stm32/board_identity.c +++ b/pkgs/board_identity/stm32/board_identity.c @@ -13,7 +13,6 @@ * Implementation of STM32 based Board identity API */ -// #include #include #include diff --git a/pkgs/board_identity/stm32/board_mcu_version.c b/pkgs/board_identity/stm32/board_mcu_version.c index 13d9367227..2c00e98e20 100644 --- a/pkgs/board_identity/stm32/board_mcu_version.c +++ b/pkgs/board_identity/stm32/board_mcu_version.c @@ -13,7 +13,6 @@ * Implementation of STM32 based SoC version API */ -// #include #include diff --git a/pkgs/crypto/crypto.h b/pkgs/crypto/crypto.h index 94016e0fa6..7b4d282d5e 100644 --- a/pkgs/crypto/crypto.h +++ b/pkgs/crypto/crypto.h @@ -35,55 +35,54 @@ #ifdef PX4_CRYPTO -#include -#include +# include +# include -#include -#include -#include -#include "crypto_backend_definitions.h" +# include +# include + +# include "crypto_backend_definitions.h" /* * Crypto API interface class */ -class PX4Crypto -{ +class PX4Crypto { public: - /* + /* * Constructor & destructor */ - PX4Crypto(); - ~PX4Crypto(); + PX4Crypto(); + ~PX4Crypto(); - /* + /* * Class function for crypto api initialization, called only once at * boot */ - static void px4_crypto_init(void); + static void px4_crypto_init(void); - /* + /* * Open crypto API for a specific algorithm * algorithm: The crypto algorithm to be used * returns true on success, false on failure */ - bool open(px4_crypto_algorithm_t algorithm); + bool open(px4_crypto_algorithm_t algorithm); - /* + /* * Close the crypto API. Optional, it is also closed by destructing the * interface object */ - void close(); + void close(); - /* + /* * Keystore access functions */ - /* + /* * Generate a single random key for symmetric-key encryption * * idx: the index in keystore where the key will be stored @@ -91,10 +90,10 @@ class PX4Crypto * returns true on success, false on failure */ - bool generate_key(uint8_t idx, - bool persistent); + bool generate_key(uint8_t idx, + bool persistent); - /* + /* * Generate a key pair for asymmetric-key encryption * * algorithm: the key type @@ -105,13 +104,13 @@ class PX4Crypto * returns true on success, false on failure */ - bool generate_keypair(size_t key_size, - uint8_t private_idx, - uint8_t public_idx, - bool persistent); + bool generate_keypair(size_t key_size, + uint8_t private_idx, + uint8_t public_idx, + bool persistent); - /* + /* * Re-create or set nonce. * * A nonce or intialization vector value for the selected algortithm is @@ -121,10 +120,10 @@ class PX4Crypto * generated */ - bool renew_nonce(const uint8_t *nonce, size_t nonce_size); + bool renew_nonce(const uint8_t *nonce, size_t nonce_size); - /* + /* * Get current crypto session nonce * * This function returns the current nonce for the session @@ -134,10 +133,10 @@ class PX4Crypto * returns true on success, false on failure */ - bool get_nonce(uint8_t *nonce, - size_t *nonce_len); + bool get_nonce(uint8_t *nonce, + size_t *nonce_len); - /* + /* * Store a key into keystore * * encryption_idx: The key index in keystore to be used in decrypting and @@ -147,11 +146,11 @@ class PX4Crypto */ - bool set_key(uint8_t encryption_idx, - const uint8_t *key, - uint8_t key_idx); + bool set_key(uint8_t encryption_idx, + const uint8_t *key, + uint8_t key_idx); - /* + /* * Get a key from keystore. Key can be encrypted * * key_idx: Index of the requested key in the keystore @@ -164,16 +163,16 @@ class PX4Crypto * */ - bool get_encrypted_key(uint8_t key_idx, - uint8_t *key, - size_t *key_len, - uint8_t encryption_key_idx); + bool get_encrypted_key(uint8_t key_idx, + uint8_t *key, + size_t *key_len, + uint8_t encryption_key_idx); - /* + /* * PX4 Crypto API functions */ - /* + /* * Verify signature * * key_index: public key index in keystore @@ -182,13 +181,13 @@ class PX4Crypto * message_size: size of the message in bytes */ - bool signature_check(uint8_t key_index, - const uint8_t *signature, - const uint8_t *message, - size_t message_size); + bool signature_check(uint8_t key_index, + const uint8_t *signature, + const uint8_t *message, + size_t message_size); - /* + /* * Encrypt data. This always supports encryption in-place * * key_index: key index in keystore @@ -200,22 +199,29 @@ class PX4Crypto * returns true on success, false on failure */ - bool encrypt_data(uint8_t key_index, - const uint8_t *message, - size_t message_size, - uint8_t *cipher, - size_t *cipher_size); + bool encrypt_data(uint8_t key_index, + const uint8_t *message, + size_t message_size, + uint8_t *cipher, + size_t *cipher_size); - size_t get_min_blocksize(uint8_t key_idx); + size_t get_min_blocksize(uint8_t key_idx); - static int crypto_ioctl(unsigned int cmd, unsigned long arg); + static int crypto_ioctl(unsigned int cmd, unsigned long arg); private: - crypto_session_handle_t _crypto_handle; - static px4_sem_t _lock; - static bool _initialized; - static void lock() { do {} while (px4_sem_wait(&PX4Crypto::_lock) != 0); } - static void unlock() { px4_sem_post(&PX4Crypto::_lock); } + crypto_session_handle_t _crypto_handle; + static px4_sem_t _lock; + static bool _initialized; + + static void lock() { + do { + } while (px4_sem_wait(&PX4Crypto::_lock) != 0); + } + + static void unlock() { + px4_sem_post(&PX4Crypto::_lock); + } }; #endif diff --git a/pkgs/device/cdev/posix/cdev_platform.cpp b/pkgs/device/cdev/posix/cdev_platform.cpp index 94a4ee5c3d..eca9021074 100644 --- a/pkgs/device/cdev/posix/cdev_platform.cpp +++ b/pkgs/device/cdev/posix/cdev_platform.cpp @@ -12,9 +12,8 @@ #include "../CDev.hpp" -#include -#include -#include +#include + #include diff --git a/pkgs/device/cdev/test/cdevtest_example.cpp b/pkgs/device/cdev/test/cdevtest_example.cpp index 85bf548528..7922ca1348 100644 --- a/pkgs/device/cdev/test/cdevtest_example.cpp +++ b/pkgs/device/cdev/test/cdevtest_example.cpp @@ -41,10 +41,8 @@ #include "cdevtest_example.h" -#include -#include -#include -#include +#include +#include #include #include diff --git a/pkgs/device/cdev/test/cdevtest_start.cpp b/pkgs/device/cdev/test/cdevtest_start.cpp index 7c83d5b4fe..41ca376e2c 100644 --- a/pkgs/device/cdev/test/cdevtest_start.cpp +++ b/pkgs/device/cdev/test/cdevtest_start.cpp @@ -40,7 +40,7 @@ #include "cdevtest_example.h" #include -#include + #include #include diff --git a/pkgs/device/drv-device/CDev.cpp b/pkgs/device/drv-device/CDev.cpp index 748626eaf7..ba3df37690 100644 --- a/pkgs/device/drv-device/CDev.cpp +++ b/pkgs/device/drv-device/CDev.cpp @@ -19,7 +19,6 @@ #include #include -#include namespace device { diff --git a/pkgs/device/drv-device/CDev.hpp b/pkgs/device/drv-device/CDev.hpp index c231a6c850..54a5e7d5f9 100644 --- a/pkgs/device/drv-device/CDev.hpp +++ b/pkgs/device/drv-device/CDev.hpp @@ -20,9 +20,6 @@ #include "Device.hpp" #include -//#include -#include - /** * Namespace encapsulating all device framework classes, functions and data. */ diff --git a/pkgs/device/drv-device/Device.hpp b/pkgs/device/drv-device/Device.hpp index 5a1172b3fd..08fb40b848 100644 --- a/pkgs/device/drv-device/Device.hpp +++ b/pkgs/device/drv-device/Device.hpp @@ -23,8 +23,7 @@ * Includes here should only cover the needs of the framework definitions. */ #include -// #include -// #include + #include #define DEVICE_LOG(FMT, ...) PX4_LOG_NAMED(_name, FMT, ##__VA_ARGS__) diff --git a/pkgs/device/drv-device/nuttx/SPI.cpp b/pkgs/device/drv-device/nuttx/SPI.cpp index 9eefc81020..c3952bcc8b 100644 --- a/pkgs/device/drv-device/nuttx/SPI.cpp +++ b/pkgs/device/drv-device/nuttx/SPI.cpp @@ -20,7 +20,7 @@ #if defined(CONFIG_SPI) # include -//#include + # include # ifndef CONFIG_SPI_EXCHANGE diff --git a/pkgs/device/drv-device/posix/SPI.cpp b/pkgs/device/drv-device/posix/SPI.cpp index 5615e31813..f5763262ed 100644 --- a/pkgs/device/drv-device/posix/SPI.cpp +++ b/pkgs/device/drv-device/posix/SPI.cpp @@ -29,8 +29,6 @@ # include -//#include - namespace device { SPI::SPI(uint8_t device_type, const char *name, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency) : diff --git a/pkgs/device/drv-device/qurt/I2C.cpp b/pkgs/device/drv-device/qurt/I2C.cpp index 9baeac4fcc..b8f94c7842 100644 --- a/pkgs/device/drv-device/qurt/I2C.cpp +++ b/pkgs/device/drv-device/qurt/I2C.cpp @@ -21,8 +21,7 @@ #if defined(CONFIG_I2C) -# include -//#include + # include namespace device { diff --git a/pkgs/device/drv-device/qurt/SPI.cpp b/pkgs/device/drv-device/qurt/SPI.cpp index 6d92592a3a..f681984599 100644 --- a/pkgs/device/drv-device/qurt/SPI.cpp +++ b/pkgs/device/drv-device/qurt/SPI.cpp @@ -20,7 +20,7 @@ #include #include #include -//#include + #include static int (*register_interrupt_callback_func)(int (*)(int, void *, void *), void *arg) = NULL; diff --git a/pkgs/device/drv-device/qurt/uart.c b/pkgs/device/drv-device/qurt/uart.c index 240ffc643b..71dde8c0b4 100644 --- a/pkgs/device/drv-device/qurt/uart.c +++ b/pkgs/device/drv-device/qurt/uart.c @@ -1,6 +1,6 @@ #include -#include + #include "uart.h" #define UART_READ_POLL_INTERVAL_US 500 diff --git a/pkgs/load_mon/LoadMon.hpp b/pkgs/load_mon/LoadMon.hpp index fa87a21e0a..05bd6a86bc 100644 --- a/pkgs/load_mon/LoadMon.hpp +++ b/pkgs/load_mon/LoadMon.hpp @@ -11,22 +11,23 @@ #pragma once #if defined(__PX4_NUTTX) -#include +# include #endif -#include -#include -#include -#include -#include -#include -#include +#include +#include + +#include +#include +#include +#include + #include #include #include #include #if defined(__PX4_LINUX) -#include +# include #endif namespace load_mon { diff --git a/pkgs/muorb/aggregator/mUORBAggregator.cpp b/pkgs/muorb/aggregator/mUORBAggregator.cpp index 5a401bb4e9..5a331c0973 100644 --- a/pkgs/muorb/aggregator/mUORBAggregator.cpp +++ b/pkgs/muorb/aggregator/mUORBAggregator.cpp @@ -31,139 +31,143 @@ * ****************************************************************************/ -#include +#include #include "mUORBAggregator.hpp" const bool mUORB::Aggregator::debugFlag = false; -bool mUORB::Aggregator::NewRecordOverflows(const char *messageName, int32_t length) -{ - if (! messageName) { return false; } +bool mUORB::Aggregator::NewRecordOverflows(const char *messageName, int32_t length) { + if (!messageName) { + return false; + } - uint32_t messageNameLength = strlen(messageName); - uint32_t newMessageRecordTotalLength = headerSize + messageNameLength + length; - return ((bufferWriteIndex + newMessageRecordTotalLength) > bufferSize); + uint32_t messageNameLength = strlen(messageName); + uint32_t newMessageRecordTotalLength = headerSize + messageNameLength + length; + return ((bufferWriteIndex + newMessageRecordTotalLength) > bufferSize); } -void mUORB::Aggregator::MoveToNextBuffer() -{ - bufferWriteIndex = 0; - bufferId++; - bufferId %= numBuffers; +void mUORB::Aggregator::MoveToNextBuffer() { + bufferWriteIndex = 0; + bufferId++; + bufferId %= numBuffers; } -void mUORB::Aggregator::AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data) -{ - if (! messageName) { return; } - - uint32_t messageNameLength = strlen(messageName); - memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &syncFlag, syncFlagSize); - bufferWriteIndex += syncFlagSize; - memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &messageNameLength, topicNameLengthSize); - bufferWriteIndex += topicNameLengthSize; - memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) &length, dataLengthSize); - bufferWriteIndex += dataLengthSize; - memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *) messageName, messageNameLength); - bufferWriteIndex += messageNameLength; - memcpy(&buffer[bufferId][bufferWriteIndex], data, length); - bufferWriteIndex += length; +void mUORB::Aggregator::AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data) { + if (!messageName) { + return; + } + + uint32_t messageNameLength = strlen(messageName); + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *)&syncFlag, syncFlagSize); + bufferWriteIndex += syncFlagSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *)&messageNameLength, topicNameLengthSize); + bufferWriteIndex += topicNameLengthSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *)&length, dataLengthSize); + bufferWriteIndex += dataLengthSize; + memcpy(&buffer[bufferId][bufferWriteIndex], (uint8_t *)messageName, messageNameLength); + bufferWriteIndex += messageNameLength; + memcpy(&buffer[bufferId][bufferWriteIndex], data, length); + bufferWriteIndex += length; } -int16_t mUORB::Aggregator::SendData() -{ - int16_t rc = 0; +int16_t mUORB::Aggregator::SendData() { + int16_t rc = 0; - if (sendFunc) { - if (aggregationEnabled) { - if (bufferWriteIndex) { - rc = sendFunc(topicName.c_str(), buffer[bufferId], bufferWriteIndex); - MoveToNextBuffer(); - } - } - } + if (sendFunc) { + if (aggregationEnabled) { + if (bufferWriteIndex) { + rc = sendFunc(topicName.c_str(), buffer[bufferId], bufferWriteIndex); + MoveToNextBuffer(); + } + } + } - return rc; + return rc; } -int16_t mUORB::Aggregator::ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) -{ - int16_t rc = 0; +int16_t mUORB::Aggregator::ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) { + int16_t rc = 0; - if (sendFunc) { - if (aggregationEnabled) { - if (NewRecordOverflows(topic, length_in_bytes)) { - rc = SendData(); - } + if (sendFunc) { + if (aggregationEnabled) { + if (NewRecordOverflows(topic, length_in_bytes)) { + rc = SendData(); + } - AddRecordToBuffer(topic, length_in_bytes, data); + AddRecordToBuffer(topic, length_in_bytes, data); - } else if (topic) { - rc = sendFunc(topic, data, length_in_bytes); - } - } + } else if (topic) { + rc = sendFunc(topic, data, length_in_bytes); + } + } - return rc; + return rc; } -void mUORB::Aggregator::ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) -{ - if (isAggregate(topic)) { - if (debugFlag) { PX4_INFO("Parsing aggregate buffer of length %u", length_in_bytes); } +void mUORB::Aggregator::ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes) { + if (isAggregate(topic)) { + if (debugFlag) { + PX4_INFO("Parsing aggregate buffer of length %u", length_in_bytes); + } - uint32_t current_index = 0; - const uint32_t name_buffer_length = 80; - char name_buffer[name_buffer_length]; + uint32_t current_index = 0; + const uint32_t name_buffer_length = 80; + char name_buffer[name_buffer_length]; - while ((current_index + headerSize) < length_in_bytes) { - uint32_t sync_flag = *((uint32_t *) &data[current_index]); + while ((current_index + headerSize) < length_in_bytes) { + uint32_t sync_flag = *((uint32_t *)&data[current_index]); - if (sync_flag != syncFlag) { - PX4_ERR("Expected sync flag but got 0x%X", sync_flag); - break; - } + if (sync_flag != syncFlag) { + PX4_ERR("Expected sync flag but got 0x%X", sync_flag); + break; + } - current_index += syncFlagSize; + current_index += syncFlagSize; - uint32_t name_length = *((uint32_t *) &data[current_index]); + uint32_t name_length = *((uint32_t *)&data[current_index]); - // Make sure name plus a terminating null can fit into our buffer - if (name_length > (name_buffer_length - 1)) { - PX4_ERR("Name length too long %u", name_length); - break; - } + // Make sure name plus a terminating null can fit into our buffer + if (name_length > (name_buffer_length - 1)) { + PX4_ERR("Name length too long %u", name_length); + break; + } - current_index += topicNameLengthSize; + current_index += topicNameLengthSize; - uint32_t data_length = *((uint32_t *) &data[current_index]); - current_index += dataLengthSize; + uint32_t data_length = *((uint32_t *)&data[current_index]); + current_index += dataLengthSize; - int32_t payload_size = name_length + data_length; - int32_t remaining_bytes = length_in_bytes - current_index; + int32_t payload_size = name_length + data_length; + int32_t remaining_bytes = length_in_bytes - current_index; - if (payload_size > remaining_bytes) { - PX4_ERR("Payload too big %u. Remaining bytes %d", payload_size, remaining_bytes); - break; - } + if (payload_size > remaining_bytes) { + PX4_ERR("Payload too big %u. Remaining bytes %d", payload_size, remaining_bytes); + break; + } - memcpy(name_buffer, &data[current_index], name_length); - name_buffer[name_length] = 0; + memcpy(name_buffer, &data[current_index], name_length); + name_buffer[name_length] = 0; - current_index += name_length; + current_index += name_length; - if (debugFlag) { PX4_INFO("Parsed topic: %s, name length %u, data length: %u", name_buffer, name_length, data_length); } + if (debugFlag) { + PX4_INFO("Parsed topic: %s, name length %u, data length: %u", name_buffer, name_length, data_length); + } - _RxHandler->process_received_message(name_buffer, - data_length, - const_cast(&data[current_index])); - current_index += data_length; - } + _RxHandler->process_received_message(name_buffer, + data_length, + const_cast(&data[current_index])); + current_index += data_length; + } - } else { - if (debugFlag) { PX4_INFO("Got non-aggregate buffer for topic %s", topic); } + } else { + if (debugFlag) { + PX4_INFO("Got non-aggregate buffer for topic %s", topic); + } - // It isn't an aggregated buffer so just process normally - _RxHandler->process_received_message(topic, - length_in_bytes, - const_cast(data)); - } + // It isn't an aggregated buffer so just process normally + _RxHandler->process_received_message(topic, + length_in_bytes, + const_cast(data)); + } } diff --git a/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp b/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp index 7d280a460b..301c3f325a 100644 --- a/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp +++ b/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp @@ -38,41 +38,37 @@ #include #include -#include +#include #include "MUORBTest.hpp" #include "uORB/uORBCommunicator.hpp" #include "mUORBAggregator.hpp" -namespace uORB -{ +namespace uORB { class AppsProtobufChannel; } -class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel -{ +class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel { public: - /** + /** * static method to get the IChannel Implementor. */ - static uORB::AppsProtobufChannel *GetInstance() - { - if (_InstancePtr == nullptr) { - _InstancePtr = new uORB::AppsProtobufChannel(); - } + static uORB::AppsProtobufChannel *GetInstance() { + if (_InstancePtr == nullptr) { + _InstancePtr = new uORB::AppsProtobufChannel(); + } - return _InstancePtr; - } + return _InstancePtr; + } - /** + /** * Static method to check if there is an instance. */ - static bool isInstance() - { - return (_InstancePtr != nullptr); - } + static bool isInstance() { + return (_InstancePtr != nullptr); + } - /** + /** * @brief Method used to initialize the fc sensor callback on the apps side. * * @param enable_debug @@ -82,9 +78,9 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel * 1 = success; This means the initialization of the fc_sensor data has happened. * otherwise = failure. */ - bool Initialize(bool enable_debug); + bool Initialize(bool enable_debug); - /** + /** * @brief Interface to notify the remote entity of a topic being advertised. * * @param messageName @@ -95,9 +91,9 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - int16_t topic_advertised(const char *messageName); + int16_t topic_advertised(const char *messageName); - /** + /** * @brief Interface to notify the remote entity of interest of a * subscription for a message. * @@ -111,9 +107,9 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - int16_t add_subscription(const char *messageName, int msgRateInHz); + int16_t add_subscription(const char *messageName, int msgRateInHz); - /** + /** * @brief Interface to notify the remote entity of removal of a subscription * * @param messageName @@ -124,14 +120,14 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - int16_t remove_subscription(const char *messageName); + int16_t remove_subscription(const char *messageName); - /** + /** * Register Message Handler. This is internal for the IChannel implementer* */ - int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); - /** + /** * @brief Sends the data message over the communication link. * @param messageName * This represents the uORB message name; This message name should be @@ -145,49 +141,48 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - int16_t send_message(const char *messageName, int length, uint8_t *data); + int16_t send_message(const char *messageName, int length, uint8_t *data); - /** + /** * @brief Interface to test the functions of the protobuf channel. * * @return * 1 = success; This means all callbacks have been tested and return the correct value. * otherwise = failure. */ - bool Test(); + bool Test(); private: - /** + /** * Data Members */ - static uORB::AppsProtobufChannel *_InstancePtr; - static uORBCommunicator::IChannelRxHandler *_RxHandler; - static mUORB::Aggregator _Aggregator; - static std::map _SlpiSubscriberCache; - static pthread_mutex_t _tx_mutex; - static pthread_mutex_t _rx_mutex; - static bool _Debug; + static uORB::AppsProtobufChannel *_InstancePtr; + static uORBCommunicator::IChannelRxHandler *_RxHandler; + static mUORB::Aggregator _Aggregator; + static std::map _SlpiSubscriberCache; + static pthread_mutex_t _tx_mutex; + static pthread_mutex_t _rx_mutex; + static bool _Debug; - bool _Initialized; - uint32_t _MessageCounter; + bool _Initialized; + uint32_t _MessageCounter; private: - /** + /** * Class Members */ - AppsProtobufChannel() {}; + AppsProtobufChannel(){}; - bool Test(MUORBTestType test_type); + bool Test(MUORBTestType test_type); - static bool test_flag; - - static void ReceiveCallback(const char *topic, - const uint8_t *data, - uint32_t length_in_bytes); - static void AdvertiseCallback(const char *topic); - static void SubscribeCallback(const char *topic); - static void UnsubscribeCallback(const char *topic); + static bool test_flag; + static void ReceiveCallback(const char *topic, + const uint8_t *data, + uint32_t length_in_bytes); + static void AdvertiseCallback(const char *topic); + static void SubscribeCallback(const char *topic); + static void UnsubscribeCallback(const char *topic); }; #endif diff --git a/pkgs/muorb/slpi/uORBProtobufChannel.cpp b/pkgs/muorb/slpi/uORBProtobufChannel.cpp index 64026bfa2f..d996a5bd48 100644 --- a/pkgs/muorb/slpi/uORBProtobufChannel.cpp +++ b/pkgs/muorb/slpi/uORBProtobufChannel.cpp @@ -36,15 +36,15 @@ #include "MUORBTest.hpp" #include -#include +#include #include #include #include #include -#include -#include -#include -#include + +#include +#include +#include #include #include "hrt_work.h" @@ -55,424 +55,419 @@ static MUORBTestType test_to_run; fc_func_ptrs muorb_func_ptrs; // static initialization. -uORB::ProtobufChannel uORB::ProtobufChannel::_Instance; +uORB::ProtobufChannel uORB::ProtobufChannel::_Instance; uORBCommunicator::IChannelRxHandler *uORB::ProtobufChannel::_RxHandler; -mUORB::Aggregator uORB::ProtobufChannel::_Aggregator; -std::map uORB::ProtobufChannel::_AppsSubscriberCache; -pthread_mutex_t uORB::ProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER; -pthread_mutex_t uORB::ProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER; +mUORB::Aggregator uORB::ProtobufChannel::_Aggregator; +std::map uORB::ProtobufChannel::_AppsSubscriberCache; +pthread_mutex_t uORB::ProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER; +pthread_mutex_t uORB::ProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER; -bool uORB::ProtobufChannel::_debug = false; -bool _px4_muorb_debug = false; -static bool px4muorb_orb_initialized = false; +bool uORB::ProtobufChannel::_debug = false; +bool _px4_muorb_debug = false; +static bool px4muorb_orb_initialized = false; // Thread for aggregator checking -qurt_thread_t aggregator_tid; +qurt_thread_t aggregator_tid; qurt_thread_attr_t aggregator_attr; // 1 is highest priority, 255 is lowest. Set it low. const uint32_t aggregator_thread_priority = 240; -const uint32_t aggregator_stack_size = 8096; -char aggregator_stack[aggregator_stack_size]; +const uint32_t aggregator_stack_size = 8096; +char aggregator_stack[aggregator_stack_size]; -static void aggregator_thread_func(void *ptr) -{ - PX4_INFO("muorb aggregator thread running"); +static void aggregator_thread_func(void *ptr) { + PX4_INFO("muorb aggregator thread running"); - uORB::ProtobufChannel *muorb = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *muorb = uORB::ProtobufChannel::GetInstance(); - while (true) { - // Check for timeout. Send buffer if timeout happened. - muorb->SendAggregateData(); + while (true) { + // Check for timeout. Send buffer if timeout happened. + muorb->SendAggregateData(); - qurt_timer_sleep(2000); - } + qurt_timer_sleep(2000); + } - qurt_thread_exit(QURT_EOK); + qurt_thread_exit(QURT_EOK); } -int16_t uORB::ProtobufChannel::topic_advertised(const char *messageName) -{ - if (_debug) { PX4_INFO("Advertising %s on remote side", messageName); } +int16_t uORB::ProtobufChannel::topic_advertised(const char *messageName) { + if (_debug) { + PX4_INFO("Advertising %s on remote side", messageName); + } - if (muorb_func_ptrs.advertise_func_ptr) { - pthread_mutex_lock(&_tx_mutex); - int16_t rc = muorb_func_ptrs.advertise_func_ptr(messageName); - pthread_mutex_unlock(&_tx_mutex); - return rc; - } + if (muorb_func_ptrs.advertise_func_ptr) { + pthread_mutex_lock(&_tx_mutex); + int16_t rc = muorb_func_ptrs.advertise_func_ptr(messageName); + pthread_mutex_unlock(&_tx_mutex); + return rc; + } - PX4_ERR("advertise_func_ptr is null in %s", __FUNCTION__); - return -1; + PX4_ERR("advertise_func_ptr is null in %s", __FUNCTION__); + return -1; } -int16_t uORB::ProtobufChannel::add_subscription(const char *messageName, int32_t msgRateInHz) -{ - // MsgRateInHz is unused in this function. - if (_debug) { PX4_INFO("Subscribing to %s on remote side", messageName); } - - if (muorb_func_ptrs.subscribe_func_ptr) { - pthread_mutex_lock(&_tx_mutex); - int16_t rc = muorb_func_ptrs.subscribe_func_ptr(messageName); - pthread_mutex_unlock(&_tx_mutex); - return rc; - } - - PX4_ERR("subscribe_func_ptr is null in %s", __FUNCTION__); - return -1; +int16_t uORB::ProtobufChannel::add_subscription(const char *messageName, int32_t msgRateInHz) { + // MsgRateInHz is unused in this function. + if (_debug) { + PX4_INFO("Subscribing to %s on remote side", messageName); + } + + if (muorb_func_ptrs.subscribe_func_ptr) { + pthread_mutex_lock(&_tx_mutex); + int16_t rc = muorb_func_ptrs.subscribe_func_ptr(messageName); + pthread_mutex_unlock(&_tx_mutex); + return rc; + } + + PX4_ERR("subscribe_func_ptr is null in %s", __FUNCTION__); + return -1; } -int16_t uORB::ProtobufChannel::remove_subscription(const char *messageName) -{ - if (_debug) { PX4_INFO("Unsubscribing from %s on remote side", messageName); } +int16_t uORB::ProtobufChannel::remove_subscription(const char *messageName) { + if (_debug) { + PX4_INFO("Unsubscribing from %s on remote side", messageName); + } - if (muorb_func_ptrs.unsubscribe_func_ptr) { - pthread_mutex_lock(&_tx_mutex); - int16_t rc = muorb_func_ptrs.unsubscribe_func_ptr(messageName); - pthread_mutex_unlock(&_tx_mutex); - return rc; - } + if (muorb_func_ptrs.unsubscribe_func_ptr) { + pthread_mutex_lock(&_tx_mutex); + int16_t rc = muorb_func_ptrs.unsubscribe_func_ptr(messageName); + pthread_mutex_unlock(&_tx_mutex); + return rc; + } - PX4_ERR("unsubscribe_func_ptr is null in %s", __FUNCTION__); - return -1; + PX4_ERR("unsubscribe_func_ptr is null in %s", __FUNCTION__); + return -1; } -int16_t uORB::ProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) -{ - _RxHandler = handler; - _Aggregator.RegisterHandler(handler); - return 0; +int16_t uORB::ProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) { + _RxHandler = handler; + _Aggregator.RegisterHandler(handler); + return 0; } -int16_t uORB::ProtobufChannel::send_message(const char *messageName, int32_t length, uint8_t *data) -{ - // This function can be called from the PX4 log function so we have to make - // sure that we do not call PX4_INFO, PX4_ERR, etc. That would cause an - // infinite loop! - bool is_not_slpi_log = true; +int16_t uORB::ProtobufChannel::send_message(const char *messageName, int32_t length, uint8_t *data) { + // This function can be called from the PX4 log function so we have to make + // sure that we do not call PX4_INFO, PX4_ERR, etc. That would cause an + // infinite loop! + bool is_not_slpi_log = true; - if ((strcmp(messageName, "slpi_debug") == 0) || (strcmp(messageName, "slpi_error") == 0)) { - is_not_slpi_log = false; - } + if ((strcmp(messageName, "slpi_debug") == 0) || (strcmp(messageName, "slpi_error") == 0)) { + is_not_slpi_log = false; + } - if (muorb_func_ptrs.topic_data_func_ptr) { - if ((_debug) && (is_not_slpi_log)) { - PX4_INFO("Got message for topic %s", messageName); - } + if (muorb_func_ptrs.topic_data_func_ptr) { + if ((_debug) && (is_not_slpi_log)) { + PX4_INFO("Got message for topic %s", messageName); + } - std::string temp(messageName); - int has_subscribers = 0; - pthread_mutex_lock(&_rx_mutex); - has_subscribers = _AppsSubscriberCache[temp]; - pthread_mutex_unlock(&_rx_mutex); + std::string temp(messageName); + int has_subscribers = 0; + pthread_mutex_lock(&_rx_mutex); + has_subscribers = _AppsSubscriberCache[temp]; + pthread_mutex_unlock(&_rx_mutex); - if ((has_subscribers) || (is_not_slpi_log == false)) { - if ((_debug) && (is_not_slpi_log)) { - PX4_INFO("Sending message for topic %s", messageName); - } + if ((has_subscribers) || (is_not_slpi_log == false)) { + if ((_debug) && (is_not_slpi_log)) { + PX4_INFO("Sending message for topic %s", messageName); + } - int16_t rc = 0; - pthread_mutex_lock(&_tx_mutex); + int16_t rc = 0; + pthread_mutex_lock(&_tx_mutex); - if (is_not_slpi_log) { - rc = _Aggregator.ProcessTransmitTopic(messageName, data, length); + if (is_not_slpi_log) { + rc = _Aggregator.ProcessTransmitTopic(messageName, data, length); - } else { - // SLPI logs don't go through the aggregator - rc = muorb_func_ptrs.topic_data_func_ptr(messageName, data, length); - } + } else { + // SLPI logs don't go through the aggregator + rc = muorb_func_ptrs.topic_data_func_ptr(messageName, data, length); + } - pthread_mutex_unlock(&_tx_mutex); - return rc; - } + pthread_mutex_unlock(&_tx_mutex); + return rc; + } - if ((_debug) && (is_not_slpi_log)) { - PX4_INFO("Skipping message for topic %s", messageName); - } + if ((_debug) && (is_not_slpi_log)) { + PX4_INFO("Skipping message for topic %s", messageName); + } - return 0; - } + return 0; + } - if (is_not_slpi_log) { - PX4_ERR("topic_data_func_ptr is null in %s", __FUNCTION__); - } + if (is_not_slpi_log) { + PX4_ERR("topic_data_func_ptr is null in %s", __FUNCTION__); + } - return -1; + return -1; } -static void *test_runner(void *) -{ - if (_px4_muorb_debug) { PX4_INFO("test_runner called"); } +static void *test_runner(void *) { + if (_px4_muorb_debug) { + PX4_INFO("test_runner called"); + } - uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); - switch (test_to_run) { - case ADVERTISE_TEST_TYPE: - (void) channel->topic_advertised(muorb_test_topic_name); - break; + switch (test_to_run) { + case ADVERTISE_TEST_TYPE: + (void)channel->topic_advertised(muorb_test_topic_name); + break; - case SUBSCRIBE_TEST_TYPE: - (void) channel->add_subscription(muorb_test_topic_name, 1); - break; + case SUBSCRIBE_TEST_TYPE: + (void)channel->add_subscription(muorb_test_topic_name, 1); + break; - case UNSUBSCRIBE_TEST_TYPE: - (void) channel->remove_subscription(muorb_test_topic_name); - break; + case UNSUBSCRIBE_TEST_TYPE: + (void)channel->remove_subscription(muorb_test_topic_name); + break; - case TOPIC_TEST_TYPE: { - uint8_t data[MUORB_TEST_DATA_LEN]; + case TOPIC_TEST_TYPE: { + uint8_t data[MUORB_TEST_DATA_LEN]; - for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) { - data[i] = i; - } + for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) { + data[i] = i; + } - (void) muorb_func_ptrs.topic_data_func_ptr(muorb_test_topic_name, data, MUORB_TEST_DATA_LEN); - } + (void)muorb_func_ptrs.topic_data_func_ptr(muorb_test_topic_name, data, MUORB_TEST_DATA_LEN); + } - default: - break; - } + default: + break; + } - return nullptr; + return nullptr; } __BEGIN_DECLS extern int slpi_main(int argc, char *argv[]); __END_DECLS -int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) -{ - hrt_set_absolute_time_offset(clock_offset_us); +int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) { + hrt_set_absolute_time_offset(clock_offset_us); - if (! px4muorb_orb_initialized) { - if (func_ptrs != nullptr) { - muorb_func_ptrs = *func_ptrs; + if (!px4muorb_orb_initialized) { + if (func_ptrs != nullptr) { + muorb_func_ptrs = *func_ptrs; - } else { - PX4_ERR("NULL top level function pointer in %s", __FUNCTION__); - return -1; - } + } else { + PX4_ERR("NULL top level function pointer in %s", __FUNCTION__); + return -1; + } - if ((muorb_func_ptrs.advertise_func_ptr == NULL) || - (muorb_func_ptrs.subscribe_func_ptr == NULL) || - (muorb_func_ptrs.unsubscribe_func_ptr == NULL) || - (muorb_func_ptrs.topic_data_func_ptr == NULL) || - (muorb_func_ptrs._config_spi_bus_func_t == NULL) || - (muorb_func_ptrs._spi_transfer_func_t == NULL) || - (muorb_func_ptrs._config_i2c_bus_func_t == NULL) || - (muorb_func_ptrs._set_i2c_address_func_t == NULL) || - (muorb_func_ptrs._i2c_transfer_func_t == NULL) || - (muorb_func_ptrs.open_uart_func_t == NULL) || - (muorb_func_ptrs.write_uart_func_t == NULL) || - (muorb_func_ptrs.read_uart_func_t == NULL) || - (muorb_func_ptrs.register_interrupt_callback == NULL)) { - PX4_ERR("NULL function pointers in %s", __FUNCTION__); - return -1; - } + if ((muorb_func_ptrs.advertise_func_ptr == NULL) || (muorb_func_ptrs.subscribe_func_ptr == NULL) || (muorb_func_ptrs.unsubscribe_func_ptr == NULL) || (muorb_func_ptrs.topic_data_func_ptr == NULL) || (muorb_func_ptrs._config_spi_bus_func_t == NULL) || (muorb_func_ptrs._spi_transfer_func_t == NULL) || (muorb_func_ptrs._config_i2c_bus_func_t == NULL) || (muorb_func_ptrs._set_i2c_address_func_t == NULL) || (muorb_func_ptrs._i2c_transfer_func_t == NULL) || (muorb_func_ptrs.open_uart_func_t == NULL) || (muorb_func_ptrs.write_uart_func_t == NULL) || (muorb_func_ptrs.read_uart_func_t == NULL) || (muorb_func_ptrs.register_interrupt_callback == NULL)) { + PX4_ERR("NULL function pointers in %s", __FUNCTION__); + return -1; + } - hrt_init(); + hrt_init(); - uORB::Manager::initialize(); - uORB::Manager::get_instance()->set_uorb_communicator( - uORB::ProtobufChannel::GetInstance()); + uORB::Manager::initialize(); + uORB::Manager::get_instance()->set_uorb_communicator( + uORB::ProtobufChannel::GetInstance()); - param_init(); + param_init(); - px4::WorkQueueManagerStart(); + px4::WorkQueueManagerStart(); - uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr); + uORB::ProtobufChannel::GetInstance()->RegisterSendHandler(muorb_func_ptrs.topic_data_func_ptr); - // Configure the I2C driver function pointers - device::I2C::configure_callbacks(muorb_func_ptrs._config_i2c_bus_func_t, muorb_func_ptrs._set_i2c_address_func_t, - muorb_func_ptrs._i2c_transfer_func_t); + // Configure the I2C driver function pointers + device::I2C::configure_callbacks(muorb_func_ptrs._config_i2c_bus_func_t, muorb_func_ptrs._set_i2c_address_func_t, + muorb_func_ptrs._i2c_transfer_func_t); - // Configure the SPI driver function pointers - device::SPI::configure_callbacks(muorb_func_ptrs._config_spi_bus_func_t, muorb_func_ptrs._spi_transfer_func_t); + // Configure the SPI driver function pointers + device::SPI::configure_callbacks(muorb_func_ptrs._config_spi_bus_func_t, muorb_func_ptrs._spi_transfer_func_t); - // Configure the UART driver function pointers - configure_uart_callbacks(muorb_func_ptrs.open_uart_func_t, muorb_func_ptrs.write_uart_func_t, - muorb_func_ptrs.read_uart_func_t); + // Configure the UART driver function pointers + configure_uart_callbacks(muorb_func_ptrs.open_uart_func_t, muorb_func_ptrs.write_uart_func_t, + muorb_func_ptrs.read_uart_func_t); - // Initialize the interrupt callback registration - register_interrupt_callback_initalizer(muorb_func_ptrs.register_interrupt_callback); + // Initialize the interrupt callback registration + register_interrupt_callback_initalizer(muorb_func_ptrs.register_interrupt_callback); - work_queues_init(); - hrt_work_queue_init(); + work_queues_init(); + hrt_work_queue_init(); - const char *argv[3] = { "slpi", "start" }; - int argc = 2; + const char *argv[3] = {"slpi", "start"}; + int argc = 2; - // Make sure that argv has a NULL pointer in the end. - argv[argc] = NULL; + // Make sure that argv has a NULL pointer in the end. + argv[argc] = NULL; - if (slpi_main(argc, (char **) argv)) { - PX4_ERR("slpi failed in %s", __FUNCTION__); - } + if (slpi_main(argc, (char **)argv)) { + PX4_ERR("slpi failed in %s", __FUNCTION__); + } - // Setup the thread to monitor for topic aggregator timeouts - qurt_thread_attr_init(&aggregator_attr); - qurt_thread_attr_set_stack_addr(&aggregator_attr, aggregator_stack); - qurt_thread_attr_set_stack_size(&aggregator_attr, aggregator_stack_size); - char thread_name[QURT_THREAD_ATTR_NAME_MAXLEN]; - strncpy(thread_name, "PX4_muorb_agg", QURT_THREAD_ATTR_NAME_MAXLEN); - qurt_thread_attr_set_name(&aggregator_attr, thread_name); - qurt_thread_attr_set_priority(&aggregator_attr, aggregator_thread_priority); - (void) qurt_thread_create(&aggregator_tid, &aggregator_attr, aggregator_thread_func, NULL); + // Setup the thread to monitor for topic aggregator timeouts + qurt_thread_attr_init(&aggregator_attr); + qurt_thread_attr_set_stack_addr(&aggregator_attr, aggregator_stack); + qurt_thread_attr_set_stack_size(&aggregator_attr, aggregator_stack_size); + char thread_name[QURT_THREAD_ATTR_NAME_MAXLEN]; + strncpy(thread_name, "PX4_muorb_agg", QURT_THREAD_ATTR_NAME_MAXLEN); + qurt_thread_attr_set_name(&aggregator_attr, thread_name); + qurt_thread_attr_set_priority(&aggregator_attr, aggregator_thread_priority); + (void)qurt_thread_create(&aggregator_tid, &aggregator_attr, aggregator_thread_func, NULL); - px4muorb_orb_initialized = true; + px4muorb_orb_initialized = true; - if (_px4_muorb_debug) { PX4_INFO("px4muorb_orb_initialize called"); } - } + if (_px4_muorb_debug) { + PX4_INFO("px4muorb_orb_initialize called"); + } + } - return 0; + return 0; } #define TEST_STACK_SIZE 8192 char stack[TEST_STACK_SIZE]; -void run_test(MUORBTestType test) -{ - test_to_run = test; - (void) px4_task_spawn_cmd("test_MUORB", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 2, - 2000, - (px4_main_t)&test_runner, - nullptr); +void run_test(MUORBTestType test) { + test_to_run = test; + (void)px4_task_spawn_cmd("test_MUORB", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 2, + 2000, + (px4_main_t)&test_runner, + nullptr); } -int px4muorb_topic_advertised(const char *topic_name) -{ - if (IS_MUORB_TEST(topic_name)) { - run_test(ADVERTISE_TEST_TYPE); +int px4muorb_topic_advertised(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { + run_test(ADVERTISE_TEST_TYPE); - if (_px4_muorb_debug) { PX4_INFO("px4muorb_topic_advertised for muorb test called"); } + if (_px4_muorb_debug) { + PX4_INFO("px4muorb_topic_advertised for muorb test called"); + } - return 0; - } + return 0; + } - uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); - if (channel) { - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + if (channel) { + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - if (rxHandler) { - return rxHandler->process_remote_topic(topic_name); + if (rxHandler) { + return rxHandler->process_remote_topic(topic_name); - } else { - PX4_ERR("Null rx handler in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null rx handler in %s", __FUNCTION__); + } - } else { - PX4_ERR("Null channel pointer in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } - return -1; + return -1; } -int px4muorb_add_subscriber(const char *topic_name) -{ - if (IS_MUORB_TEST(topic_name)) { - run_test(SUBSCRIBE_TEST_TYPE); +int px4muorb_add_subscriber(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { + run_test(SUBSCRIBE_TEST_TYPE); - if (_px4_muorb_debug) { PX4_INFO("px4muorb_add_subscriber for muorb test called"); } + if (_px4_muorb_debug) { + PX4_INFO("px4muorb_add_subscriber for muorb test called"); + } - return 0; - } + return 0; + } - uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); - if (channel) { - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + if (channel) { + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - if (rxHandler) { - channel->AddRemoteSubscriber(topic_name); - // Pick a high message rate of 1000 Hz - return rxHandler->process_add_subscription(topic_name); + if (rxHandler) { + channel->AddRemoteSubscriber(topic_name); + // Pick a high message rate of 1000 Hz + return rxHandler->process_add_subscription(topic_name); - } else { - PX4_ERR("Null rx handler in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null rx handler in %s", __FUNCTION__); + } - } else { - PX4_ERR("Null channel pointer in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } - return -1; + return -1; } -int px4muorb_remove_subscriber(const char *topic_name) -{ - if (IS_MUORB_TEST(topic_name)) { - run_test(UNSUBSCRIBE_TEST_TYPE); +int px4muorb_remove_subscriber(const char *topic_name) { + if (IS_MUORB_TEST(topic_name)) { + run_test(UNSUBSCRIBE_TEST_TYPE); - if (_px4_muorb_debug) { PX4_INFO("px4muorb_remove_subscriber for muorb test called"); } + if (_px4_muorb_debug) { + PX4_INFO("px4muorb_remove_subscriber for muorb test called"); + } - return 0; - } + return 0; + } - uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); - if (channel) { - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + if (channel) { + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - if (rxHandler) { - channel->RemoveRemoteSubscriber(topic_name); - return rxHandler->process_remove_subscription(topic_name); + if (rxHandler) { + channel->RemoveRemoteSubscriber(topic_name); + return rxHandler->process_remove_subscription(topic_name); - } else { - PX4_ERR("Null rx handler in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null rx handler in %s", __FUNCTION__); + } - } else { - PX4_ERR("Null channel pointer in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } - return -1; + return -1; } int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data, - int data_len_in_bytes) -{ - if (IS_MUORB_TEST(topic_name)) { - // Validate the test data received - bool test_passed = true; + int data_len_in_bytes) { + if (IS_MUORB_TEST(topic_name)) { + // Validate the test data received + bool test_passed = true; - if (data_len_in_bytes != MUORB_TEST_DATA_LEN) { - test_passed = false; + if (data_len_in_bytes != MUORB_TEST_DATA_LEN) { + test_passed = false; - } else { - for (int i = 0; i < data_len_in_bytes; i++) { - if ((uint8_t) i != data[i]) { - test_passed = false; - break; - } - } - } + } else { + for (int i = 0; i < data_len_in_bytes; i++) { + if ((uint8_t)i != data[i]) { + test_passed = false; + break; + } + } + } - if (test_passed) { run_test(TOPIC_TEST_TYPE); } + if (test_passed) { + run_test(TOPIC_TEST_TYPE); + } - if (_px4_muorb_debug) { PX4_INFO("px4muorb_send_topic_data called"); } + if (_px4_muorb_debug) { + PX4_INFO("px4muorb_send_topic_data called"); + } - return 0; - } + return 0; + } - uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); - if (channel) { - uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + if (channel) { + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); - if (rxHandler) { - return rxHandler->process_received_message(topic_name, - data_len_in_bytes, - (uint8_t *) data); + if (rxHandler) { + return rxHandler->process_received_message(topic_name, + data_len_in_bytes, + (uint8_t *)data); - } else { - PX4_ERR("Null rx handler in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null rx handler in %s", __FUNCTION__); + } - } else { - PX4_ERR("Null channel pointer in %s", __FUNCTION__); - } + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } - return -1; + return -1; }