From 4f87cb0b76396bdd17280350c714ebb92a45ee2f Mon Sep 17 00:00:00 2001 From: Florian Achermann Date: Wed, 9 Jun 2021 11:05:28 +0200 Subject: [PATCH] Fix format --- src/drivers/si7210/si7210_main.cpp | 3 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- .../mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp | 64 ++++++++++--------- src/modules/sensors/sensors.cpp | 33 +++++----- 4 files changed, 56 insertions(+), 46 deletions(-) diff --git a/src/drivers/si7210/si7210_main.cpp b/src/drivers/si7210/si7210_main.cpp index 1364682713..682e9e9a1f 100644 --- a/src/drivers/si7210/si7210_main.cpp +++ b/src/drivers/si7210/si7210_main.cpp @@ -85,7 +85,8 @@ si7210_main(int argc, char *argv[]) cli.support_keep_running = true; int ch; - while ((ch = cli.getopt(argc, argv, "")) != EOF) {} + + while ((ch = cli.getopt(argc, argv, "")) != EOF) {} const char *verb = cli.parseDefaultArguments(argc, argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a0c1fbd8ad..08c17d368b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -348,7 +348,7 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // SCALED_PRESSURE3 #if defined(SENSOR_AIRFLOW_ANGLES_HPP) - create_stream_list_item(), + create_stream_list_item(), #endif // SENSOR_AIRFLOW_ANGLES #if defined(ACTUATOR_OUTPUT_STATUS_HPP) create_stream_list_item(), diff --git a/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp b/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp index bb0173b80b..5be7de3f7c 100644 --- a/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp +++ b/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp @@ -50,11 +50,11 @@ public: unsigned get_size() override { - if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) { - return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } + if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) { + return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } - return 0; + return 0; } private: @@ -66,34 +66,40 @@ private: bool send() override { if (_airflow_aoa_sub.updated() || _airflow_slip_sub.updated()) { - mavlink_sensor_airflow_angles_t msg{}; + mavlink_sensor_airflow_angles_t msg{}; - struct airflow_aoa_s airflow_aoa; - if (_airflow_aoa_sub.copy(&airflow_aoa)) { - msg.timestamp = airflow_aoa.timestamp / 1000; - msg.angleofattack = math::degrees(airflow_aoa.aoa_rad); - msg.angleofattack_valid = airflow_aoa.valid; - } else { - msg.timestamp = 0; - msg.angleofattack = 0.0; - msg.angleofattack_valid = false; - } + struct airflow_aoa_s airflow_aoa; - struct airflow_slip_s airflow_slip; - if (_airflow_slip_sub.copy(&airflow_slip)) { - const uint64_t timestamp = airflow_slip.timestamp / 1000; - if (timestamp > msg.timestamp) { - msg.timestamp = timestamp; - } - msg.sideslip = math::degrees(airflow_slip.slip_rad); - msg.sideslip_valid = airflow_slip.valid; - } else { - msg.timestamp = 0; - msg.sideslip = 0.0; - msg.sideslip_valid = false; - } + if (_airflow_aoa_sub.copy(&airflow_aoa)) { + msg.timestamp = airflow_aoa.timestamp / 1000; + msg.angleofattack = math::degrees(airflow_aoa.aoa_rad); + msg.angleofattack_valid = airflow_aoa.valid; - mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg); + } else { + msg.timestamp = 0; + msg.angleofattack = 0.0; + msg.angleofattack_valid = false; + } + + struct airflow_slip_s airflow_slip; + + if (_airflow_slip_sub.copy(&airflow_slip)) { + const uint64_t timestamp = airflow_slip.timestamp / 1000; + + if (timestamp > msg.timestamp) { + msg.timestamp = timestamp; + } + + msg.sideslip = math::degrees(airflow_slip.slip_rad); + msg.sideslip_valid = airflow_slip.valid; + + } else { + msg.timestamp = 0; + msg.sideslip = 0.0; + msg.sideslip_valid = false; + } + + mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg); return true; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7900c79f8e..df6b8a46d3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -132,15 +132,15 @@ private: uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed - {ORB_ID(sensor_hall), 0}, - {ORB_ID(sensor_hall), 1}, - {ORB_ID(sensor_hall), 2}, - {ORB_ID(sensor_hall), 3}, - }; + uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed + {ORB_ID(sensor_hall), 0}, + {ORB_ID(sensor_hall), 1}, + {ORB_ID(sensor_hall), 2}, + {ORB_ID(sensor_hall), 3}, + }; - int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription - int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription + int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription + int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; @@ -442,7 +442,7 @@ void Sensors::diff_pres_poll() } else { // differential pressure temperature invalid, check barometer if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { + && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; @@ -480,13 +480,13 @@ void Sensors::diff_pres_poll() /* don't risk to feed negative airspeed into the system */ airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) - _parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, - air_temperature_celsius); + _parameters.air_cmodel, + smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, + diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa, + air_temperature_celsius); airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa, - air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here + air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here airspeed.air_temperature_celsius = air_temperature_celsius; @@ -593,6 +593,7 @@ void Sensors::hall_poll() if ((_av_aoa_hall_sub_index >= 0) && (_av_aoa_hall_sub_index < MAX_SENSOR_COUNT)) { // A hall sensor with selected driver ID for the AoA vane has been found/set struct sensor_hall_s sensor_hall; + if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) { airflow_aoa_s airflow_aoa; airflow_aoa.timestamp = hrt_absolute_time(); @@ -624,6 +625,7 @@ void Sensors::hall_poll() if ((_av_slip_hall_sub_index >= 0) && (_av_slip_hall_sub_index < MAX_SENSOR_COUNT)) { // A hall sensor with selected driver ID for the slip vane has been found/set struct sensor_hall_s sensor_hall; + if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) { airflow_slip_s airflow_slip; airflow_slip.timestamp = hrt_absolute_time(); @@ -663,7 +665,7 @@ void Sensors::hall_poll() } // find the Slip hall sensor index if not set yet - if (_parameters.CAL_AV_SLIP_ID >= 0&& _av_slip_hall_sub_index < 0) { + if (_parameters.CAL_AV_SLIP_ID >= 0 && _av_slip_hall_sub_index < 0) { _av_slip_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_SLIP_ID); if (_av_slip_hall_sub_index >= 0) { @@ -676,6 +678,7 @@ int Sensors::getHallSubIndex(const int av_driver_id) { for (unsigned i = 0; i < MAX_SENSOR_COUNT; i++) { sensor_hall_s sensor_hall; + if (_sensor_hall_subs[i].copy(&sensor_hall)) { if (sensor_hall.instance == av_driver_id) { return i;