From 3faab909d7db25bef6dc3e2eb753cdc05f7f9ce5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 30 May 2019 13:20:54 -0400 Subject: [PATCH] commander move most orb subscriptions to uORB::Subscription --- src/modules/commander/Commander.cpp | 185 ++++++------------ src/modules/commander/Commander.hpp | 6 +- .../commander/accelerometer_calibration.cpp | 9 +- 3 files changed, 63 insertions(+), 137 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 53ba481425..af191f9968 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -558,11 +558,6 @@ Commander::Commander() : { _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); _auto_disarm_killed.set_hysteresis_time_from(false, 5_s); - _battery_sub = orb_subscribe(ORB_ID(battery_status)); - - - _telemetry_status_sub = orb_subscribe(ORB_ID(telemetry_status)); - // We want to accept RC inputs as default status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; @@ -582,20 +577,10 @@ Commander::Commander() : status_flags.rc_calibration_valid = true; status_flags.avoidance_system_valid = false; - - } Commander::~Commander() { - orb_unsubscribe(_battery_sub); - orb_unsubscribe(_telemetry_status_sub); - - - if (_iridiumsbd_status_sub >= 0) { - orb_unsubscribe(_iridiumsbd_status_sub); - } - delete[] _airspeed_fault_type; } @@ -1230,7 +1215,7 @@ Commander::run() PX4_WARN("Buzzer init failed"); } - int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state)); + uORB::Subscription power_button_state_sub{ORB_ID(power_button_state)}; { // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen // in IRQ context. @@ -1238,7 +1223,8 @@ Commander::run() button_state.timestamp = 0; button_state.event = 0xff; power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state); - orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state); + + power_button_state_sub.copy(&button_state); } if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) { @@ -1275,18 +1261,18 @@ Commander::run() bool updated = false; - int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - int cpuload_sub = orb_subscribe(ORB_ID(cpuload)); - int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result)); - int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode)); - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - int safety_sub = orb_subscribe(ORB_ID(safety)); - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - int system_power_sub = orb_subscribe(ORB_ID(system_power)); - int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); + uORB::Subscription actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; + uORB::Subscription cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)}; + uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription param_changed_sub{ORB_ID(parameter_update)}; + uORB::Subscription safety_sub{ORB_ID(safety)}; + uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription subsys_sub{ORB_ID(subsystem_info)}; + uORB::Subscription system_power_sub{ORB_ID(system_power)}; + uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; geofence_result_s geofence_result {}; @@ -1388,14 +1374,13 @@ Commander::run() transition_result_t arming_ret = TRANSITION_NOT_CHANGED; /* update parameters */ - bool params_updated = false; - orb_check(param_changed_sub, ¶ms_updated); + bool params_updated = param_changed_sub.updated(); if (params_updated || param_init_forced) { /* parameters changed */ - struct parameter_update_s param_changed; - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + parameter_update_s param_changed; + param_changed_sub.copy(¶m_changed); updateParams(); @@ -1490,28 +1475,20 @@ Commander::run() status_flags.avoidance_system_required = _param_com_obs_avoid.get(); /* handle power button state */ - orb_check(power_button_state_sub, &updated); - - if (updated) { + if (power_button_state_sub.updated()) { power_button_state_s button_state; - orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state); + power_button_state_sub.copy(&button_state); if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { px4_shutdown_request(false, false); } } - orb_check(sp_man_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } + sp_man_sub.update(&sp_man); - orb_check(offboard_control_mode_sub, &updated); - - if (updated) { + if (offboard_control_mode_sub.updated()) { offboard_control_mode_s old = offboard_control_mode; - orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode); + offboard_control_mode_sub.copy(&offboard_control_mode); if (old.ignore_thrust != offboard_control_mode.ignore_thrust || old.ignore_attitude != offboard_control_mode.ignore_attitude || @@ -1558,11 +1535,10 @@ Commander::run() } } - orb_check(system_power_sub, &updated); - if (updated) { + if (system_power_sub.updated()) { system_power_s system_power = {}; - orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + system_power_sub.copy(&system_power); if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { if (system_power.servo_valid && @@ -1589,12 +1565,10 @@ Commander::run() } /* update safety topic */ - orb_check(safety_sub, &updated); - - if (updated) { + if (safety_sub.updated()) { bool previous_safety_off = safety.safety_off; - if (orb_copy(ORB_ID(safety), safety_sub, &safety) == PX4_OK) { + if (safety_sub.copy(&safety)) { /* disarm if safety is now on and still armed */ if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF) @@ -1624,11 +1598,9 @@ Commander::run() } /* update vtol vehicle status*/ - orb_check(vtol_vehicle_status_sub, &updated); - - if (updated) { + if (vtol_vehicle_status_sub.updated()) { /* vtol status changed */ - orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); + vtol_vehicle_status_sub.copy(&vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle really is of type vtol */ @@ -1666,10 +1638,8 @@ Commander::run() airspeed_use_check(); /* Update land detector */ - orb_check(land_detector_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + if (land_detector_sub.updated()) { + land_detector_sub.copy(&land_detector); // Only take actions if armed if (armed.armed) { @@ -1749,21 +1719,15 @@ Commander::run() _geofence_warning_action_on = false; } - orb_check(cpuload_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload); - } + cpuload_sub.update(&cpuload); battery_status_check(); /* update subsystem info which arrives from outside of commander*/ do { - orb_check(subsys_sub, &updated); - - if (updated) { + if (subsys_sub.updated()) { subsystem_info_s info{}; - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + subsys_sub.copy(&info); set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status); status_changed = true; } @@ -1826,11 +1790,7 @@ Commander::run() } /* start geofence result check */ - orb_check(geofence_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); - } + geofence_result_sub.update(&geofence_result); // Geofence actions const bool geofence_action_enabled = geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE; @@ -2158,9 +2118,7 @@ Commander::run() // engine failure detection // TODO: move out of commander - orb_check(actuator_controls_sub, &updated); - - if (updated) { + if (actuator_controls_sub.updated()) { /* Check engine failure * only for fixed wing for now */ @@ -2168,7 +2126,7 @@ Commander::run() !status.is_rotary_wing && !status.is_vtol && armed.armed) { actuator_controls_s actuator_controls = {}; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); + actuator_controls_sub.copy(&actuator_controls); const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; const float current2throttle = _battery_current / throttle; @@ -2235,13 +2193,11 @@ Commander::run() } /* handle commands last, as the system needs to be updated to handle them */ - orb_check(cmd_sub, &updated); - - if (updated) { - struct vehicle_command_s cmd; + if (cmd_sub.updated()) { + vehicle_command_s cmd{}; /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + cmd_sub.copy(&cmd); /* handle it */ if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) { @@ -2498,13 +2454,6 @@ Commander::run() /* close fds */ led_deinit(); buzzer_deinit(); - orb_unsubscribe(sp_man_sub); - orb_unsubscribe(offboard_control_mode_sub); - orb_unsubscribe(safety_sub); - orb_unsubscribe(cmd_sub); - orb_unsubscribe(subsys_sub); - orb_unsubscribe(param_changed_sub); - orb_unsubscribe(land_detector_sub); thread_running = false; } @@ -3500,7 +3449,7 @@ void *commander_low_prio_loop(void *arg) continue; } else if (pret != 0) { - struct vehicle_command_s cmd; + struct vehicle_command_s cmd {}; /* if we reach here, we have a valid command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); @@ -3826,15 +3775,11 @@ bool Commander::preflight_check(bool report) void Commander::data_link_check(bool &status_changed) { - bool updated = false; - - orb_check(_telemetry_status_sub, &updated); - - if (updated) { + if (_telemetry_status_sub.updated()) { telemetry_status_s telemetry; - if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub, &telemetry) == PX4_OK) { + if (_telemetry_status_sub.copy(&telemetry)) { // handle different radio types switch (telemetry.type) { @@ -3843,38 +3788,25 @@ void Commander::data_link_check(bool &status_changed) status_flags.usb_connected = true; break; - case telemetry_status_s::LINK_TYPE_IRIDIUM: - - // lazily subscribe - if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) { - _iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status)); - } - - if (_iridiumsbd_status_sub >= 0) { - bool iridiumsbd_updated = false; - orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated); + case telemetry_status_s::LINK_TYPE_IRIDIUM: { - if (iridiumsbd_updated) { - iridiumsbd_status_s iridium_status; + iridiumsbd_status_s iridium_status; - if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) { - _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; + if (_iridiumsbd_status_sub.update(&iridium_status)) { + _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; - if (status.high_latency_data_link_lost) { - if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { - status.high_latency_data_link_lost = false; - status_changed = true; - } + if (status.high_latency_data_link_lost) { + if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { + status.high_latency_data_link_lost = false; + status_changed = true; } - } } - } - break; + break; + } } - // handle different remote types switch (telemetry.remote_type) { case telemetry_status_s::MAV_TYPE_GCS: @@ -4021,16 +3953,11 @@ void Commander::data_link_check(bool &status_changed) void Commander::battery_status_check() { - bool updated = false; - /* update battery status */ - orb_check(_battery_sub, &updated); - - if (updated) { - - battery_status_s battery = {}; + if (_battery_sub.updated()) { + battery_status_s battery{}; - if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) { + if (_battery_sub.copy(&battery)) { if ((hrt_elapsed_time(&battery.timestamp) < 5_s) && battery.connected diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d63a0f2d12..9cba70ae45 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -208,7 +208,7 @@ private: */ void data_link_check(bool &status_changed); - int _telemetry_status_sub{-1}; + uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; hrt_abstime _datalink_last_heartbeat_gcs{0}; @@ -221,12 +221,12 @@ private: bool _avoidance_system_status_change{false}; uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT}; - int _iridiumsbd_status_sub{-1}; + uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; - int _battery_sub{-1}; + uORB::Subscription _battery_sub{ORB_ID(battery_status)}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; float _battery_current{0.0f}; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 2059afddb5..3936be9a46 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -148,6 +148,7 @@ #include #include #include +#include static const char *sensor_name = "accel"; @@ -334,11 +335,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) if (tc_enabled_int == 1) { /* Get struct containing sensor thermal compensation data */ - struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ - memset(&sensor_correction, 0, sizeof(sensor_correction)); - int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); - orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction); - orb_unsubscribe(sensor_correction_sub); + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ + uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; + sensor_correction_sub.copy(&sensor_correction); /* don't allow a parameter instance to be calibrated more than once by another uORB instance */ if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {