diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 56a5b704f0..ca0d121b8d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -297,14 +297,10 @@ FixedwingPositionControl::parameters_update() void FixedwingPositionControl::vehicle_control_mode_poll() { - bool updated = false; - - orb_check(_control_mode_sub, &updated); - - if (updated) { + if (_control_mode_sub.updated()) { const bool was_armed = _control_mode.flag_armed; - if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) == PX4_OK) { + if (_control_mode_sub.copy(&_control_mode)) { // reset state when arming if (!was_armed && _control_mode.flag_armed) { @@ -318,12 +314,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() void FixedwingPositionControl::vehicle_command_poll() { - bool updated; - - orb_check(_vehicle_command_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command); + if (_vehicle_command_sub.updated()) { + _vehicle_command_sub.copy(&_vehicle_command); handle_command(); } } @@ -331,13 +323,7 @@ FixedwingPositionControl::vehicle_command_poll() void FixedwingPositionControl::vehicle_status_poll() { - bool updated; - - orb_check(_vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - + if (_vehicle_status_sub.update(&_vehicle_status)) { /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_attitude_setpoint_id == nullptr) { if (_vehicle_status.is_vtol) { @@ -355,31 +341,6 @@ FixedwingPositionControl::vehicle_status_poll() } } -void -FixedwingPositionControl::vehicle_land_detected_poll() -{ - bool updated; - - orb_check(_vehicle_land_detected_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); - } -} - -void -FixedwingPositionControl::manual_control_setpoint_poll() -{ - bool manual_updated; - - /* Check if manual setpoint has changed */ - orb_check(_manual_control_sub, &manual_updated); - - if (manual_updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); - } -} - void FixedwingPositionControl::airspeed_poll() { @@ -419,39 +380,21 @@ FixedwingPositionControl::airspeed_poll() void FixedwingPositionControl::vehicle_attitude_poll() { - /* check if there is a new position */ - bool updated; - orb_check(_vehicle_attitude_sub, &updated); + if (_vehicle_attitude_sub.update(&_att)) { + /* set rotation matrix and euler angles */ + _R_nb = Quatf(_att.q); - if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); - } + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); + _R_nb = _R_nb * R_offset; + } - /* set rotation matrix and euler angles */ - _R_nb = Quatf(_att.q); - - // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset - // between multirotor and fixed wing flight - if (static_cast(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { - Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); - _R_nb = _R_nb * R_offset; - } - - Eulerf euler_angles(_R_nb); - _roll = euler_angles(0); - _pitch = euler_angles(1); - _yaw = euler_angles(2); -} - -void -FixedwingPositionControl::position_setpoint_triplet_poll() -{ - /* check if there is a new setpoint */ - bool pos_sp_triplet_updated; - orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - - if (pos_sp_triplet_updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + Eulerf euler_angles(_R_nb); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); } } @@ -1732,20 +1675,7 @@ FixedwingPositionControl::handle_command() void FixedwingPositionControl::run() { - /* - * do subscriptions - */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -1780,13 +1710,12 @@ FixedwingPositionControl::run() } /* only update parameters if they changed */ - bool params_updated = false; - orb_check(_params_sub, ¶ms_updated); + bool params_updated = _params_sub.updated(); if (params_updated) { /* read from param to clear updated flag */ parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); + _params_sub.copy(&update); /* update parameters from storage */ parameters_update(); @@ -1798,7 +1727,7 @@ FixedwingPositionControl::run() /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + _local_pos_sub.update(&_local_pos); // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { @@ -1823,12 +1752,12 @@ FixedwingPositionControl::run() _sub_sensors.update(); airspeed_poll(); - manual_control_setpoint_poll(); - position_setpoint_triplet_poll(); + _manual_control_sub.update(&_manual); + _pos_sp_triplet_sub.update(&_pos_sp_triplet); vehicle_attitude_poll(); vehicle_command_poll(); vehicle_control_mode_poll(); - vehicle_land_detected_poll(); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); vehicle_status_poll(); Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); @@ -2023,13 +1952,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee /* scale throttle cruise by baro pressure */ if (_parameters.throttle_alt_scale > FLT_EPSILON) { + sensor_baro_s baro{}; - bool baro_updated = false; - orb_check(_sensor_baro_sub, &baro_updated); - - sensor_baro_s baro; - - if (orb_copy(ORB_ID(sensor_baro), _sensor_baro_sub, &baro) == PX4_OK) { + if (_sensor_baro_sub.update(&baro)) { if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) { // scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature) const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 3229e4a673..4f45957456 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -152,16 +152,17 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; int _global_pos_sub{-1}; - int _local_pos_sub{-1}; - int _pos_sp_triplet_sub{-1}; - int _control_mode_sub{-1}; ///< control mode subscription */ - int _vehicle_attitude_sub{-1}; ///< vehicle attitude subscription */ - int _vehicle_command_sub{-1}; ///< vehicle command subscription */ - int _vehicle_status_sub{-1}; ///< vehicle status subscription */ - int _vehicle_land_detected_sub{-1}; ///< vehicle land detected subscription */ - int _params_sub{-1}; ///< notification of parameter updates */ - int _manual_control_sub{-1}; ///< notification of manual control updates */ - int _sensor_baro_sub{-1}; + + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription */ + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates */ + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription */ + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription */ orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */ orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */ @@ -172,7 +173,7 @@ private: manual_control_setpoint_s _manual {}; ///< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items */ - vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */ + vehicle_attitude_s _att {}; ///< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint */ vehicle_command_s _vehicle_command {}; ///< vehicle commands */ vehicle_control_mode_s _control_mode {}; ///< control mode */ @@ -377,12 +378,9 @@ private: // Update subscriptions void airspeed_poll(); void control_update(); - void manual_control_setpoint_poll(); - void position_setpoint_triplet_poll(); void vehicle_attitude_poll(); void vehicle_command_poll(); void vehicle_control_mode_poll(); - void vehicle_land_detected_poll(); void vehicle_status_poll(); void status_publish();