diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c112e01ec3..0bf69bcd2e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -58,7 +58,6 @@ set(msg_files esc_status.msg estimator_status.msg follow_target.msg - fw_pos_ctrl_status.msg geofence_result.msg gps_dump.msg gps_inject_data.msg @@ -81,6 +80,8 @@ set(msg_files optical_flow.msg parameter_update.msg ping.msg + position_controller_landing_status.msg + position_controller_status.msg position_setpoint.msg position_setpoint_triplet.msg power_button_state.msg diff --git a/msg/fw_pos_ctrl_status.msg b/msg/fw_pos_ctrl_status.msg deleted file mode 100644 index cbd148b1e4..0000000000 --- a/msg/fw_pos_ctrl_status.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -float32 nav_roll -float32 nav_pitch -float32 nav_bearing - -float32 target_bearing -float32 wp_dist -float32 xtrack_error -float32 turn_distance # the optimal distance to a waypoint to switch to the next - -float32 landing_horizontal_slope_displacement -float32 landing_slope_angle_rad -float32 landing_flare_length -bool abort_landing diff --git a/msg/position_controller_landing_status.msg b/msg/position_controller_landing_status.msg new file mode 100644 index 0000000000..8f5ae9654b --- /dev/null +++ b/msg/position_controller_landing_status.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +float32 horizontal_slope_displacement + +float32 slope_angle_rad + +float32 flare_length + +bool abort_landing # true if landing should be aborted diff --git a/msg/position_controller_status.msg b/msg/position_controller_status.msg new file mode 100644 index 0000000000..43e1c0589b --- /dev/null +++ b/msg/position_controller_status.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) + +float32 nav_roll +float32 nav_pitch +float32 nav_bearing + +float32 target_bearing +float32 xtrack_error + +float32 wp_dist + +float32 acceptance_radius # the optimal distance to a waypoint to switch to the next + +float32 yaw_acceptance # NaN if not set diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 498929e357..c56eac68fc 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -277,13 +277,7 @@ FixedwingPositionControl::parameters_update() _landingslope.update(radians(land_slope_angle), land_flare_alt_relative, land_thrust_lim_alt_relative, land_H1_virt); - - // Update and publish the navigation capabilities - _fw_pos_ctrl_status.landing_slope_angle_rad = _landingslope.landing_slope_angle_rad(); - _fw_pos_ctrl_status.landing_horizontal_slope_displacement = _landingslope.horizontal_slope_displacement(); - _fw_pos_ctrl_status.landing_flare_length = _landingslope.flare_length(); - fw_pos_ctrl_status_publish(); - + landing_status_publish(); // sanity check parameters if ((_parameters.airspeed_max < _parameters.airspeed_min) || @@ -546,18 +540,121 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos } void -FixedwingPositionControl::fw_pos_ctrl_status_publish() +FixedwingPositionControl::tecs_status_publish() { - _fw_pos_ctrl_status.timestamp = hrt_absolute_time(); + tecs_status_s t = {}; - if (_fw_pos_ctrl_status_pub != nullptr) { - orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status); + switch (_tecs.tecs_mode()) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; + } + + t.altitude_sp = _tecs.hgt_setpoint_adj(); + t.altitude_filtered = _tecs.vert_pos_state(); + + t.airspeed_sp = _tecs.TAS_setpoint_adj(); + t.airspeed_filtered = _tecs.tas_state(); + + t.flight_path_angle_sp = _tecs.hgt_rate_setpoint(); + t.flight_path_angle = _tecs.vert_vel_state(); + + t.airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); + t.airspeed_derivative = _tecs.speed_derivative(); + + t.total_energy_error = _tecs.STE_error(); + t.total_energy_rate_error = _tecs.STE_rate_error(); + + t.energy_distribution_error = _tecs.SEB_error(); + t.energy_distribution_rate_error = _tecs.SEB_rate_error(); + + t.throttle_integ = _tecs.throttle_integ_state(); + t.pitch_integ = _tecs.pitch_integ_state(); + + t.timestamp = hrt_absolute_time(); + + if (_tecs_status_pub != nullptr) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); } else { - _fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status); + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); } } +void +FixedwingPositionControl::status_publish() +{ + position_controller_status_s pos_ctrl_status = {}; + + pos_ctrl_status.nav_roll = _att_sp.roll_body; + pos_ctrl_status.nav_pitch = _att_sp.pitch_body; + pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); + + pos_ctrl_status.target_bearing = _l1_control.target_bearing(); + pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); + + pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); + + pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f); + + pos_ctrl_status.yaw_acceptance = NAN; + + pos_ctrl_status.timestamp = hrt_absolute_time(); + + if (_pos_ctrl_status_pub != nullptr) { + orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status); + + } else { + _pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status); + } +} + +void +FixedwingPositionControl::landing_status_publish() +{ + position_controller_landing_status_s pos_ctrl_landing_status = {}; + + pos_ctrl_landing_status.slope_angle_rad = _landingslope.landing_slope_angle_rad(); + pos_ctrl_landing_status.horizontal_slope_displacement = _landingslope.horizontal_slope_displacement(); + pos_ctrl_landing_status.flare_length = _landingslope.flare_length(); + + pos_ctrl_landing_status.abort_landing = _land_abort; + + pos_ctrl_landing_status.timestamp = hrt_absolute_time(); + + if (_pos_ctrl_landing_status_pub != nullptr) { + orb_publish(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_pub, &pos_ctrl_landing_status); + + } else { + _pos_ctrl_landing_status_pub = orb_advertise(ORB_ID(position_controller_landing_status), &pos_ctrl_landing_status); + } +} + +void +FixedwingPositionControl::abort_landing(bool abort) +{ + // only announce changes + if (abort && !_land_abort) { + mavlink_log_critical(&_mavlink_log_pub, "Landing aborted"); + } + + _land_abort = abort; + landing_status_publish(); +} + void FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev, position_setpoint_s &waypoint_next, bool flag_init) @@ -677,7 +774,7 @@ bool FixedwingPositionControl::in_takeoff_situation() { // in air for < 10s - const hrt_abstime delta_takeoff = 10000000; + const hrt_abstime delta_takeoff = 10_s; return (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff) && (_global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff); @@ -864,10 +961,10 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f)); } - if (_fw_pos_ctrl_status.abort_landing) { + if (_land_abort) { if (pos_sp_curr.alt - _global_pos.alt < _parameters.climbout_diff) { // aborted landing complete, normal loiter over landing point - _fw_pos_ctrl_status.abort_landing = false; + abort_landing(false); } else { // continue straight until vehicle has sufficient altitude @@ -1344,9 +1441,8 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // save time at which we started landing and reset abort_landing if (_time_started_landing == 0) { + reset_landing_state(); _time_started_landing = hrt_absolute_time(); - - _fw_pos_ctrl_status.abort_landing = false; } const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), @@ -1437,16 +1533,16 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint - if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { + if (hrt_elapsed_time(&_time_started_landing) < 10_s) { terrain_alt = pos_sp_curr.alt; } else { // still no valid terrain, abort landing terrain_alt = pos_sp_curr.alt; - _fw_pos_ctrl_status.abort_landing = true; + abort_landing(true); } - } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT) || _land_noreturn_vertical) { // use previous terrain estimate for some time and hope to recover // if we are already flaring (land_noreturn_vertical) then just @@ -1456,7 +1552,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector } else { // terrain alt was not valid for long time, abort landing terrain_alt = _t_alt_prev_valid; - _fw_pos_ctrl_status.abort_landing = true; + abort_landing(true); } } @@ -1615,8 +1711,7 @@ FixedwingPositionControl::handle_command() _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - _fw_pos_ctrl_status.abort_landing = true; - mavlink_log_critical(&_mavlink_log_pub, "Landing aborted"); + abort_landing(true); } } } @@ -1760,32 +1855,11 @@ FixedwingPositionControl::run() /* advertise and publish */ _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } - } - /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(500.0f); - - /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) > 1000000) - || (fabsf(turn_distance - _fw_pos_ctrl_status.turn_distance) > FLT_EPSILON - && turn_distance > 0)) { - - /* set new turn distance */ - _fw_pos_ctrl_status.turn_distance = turn_distance; - - _fw_pos_ctrl_status.nav_roll = _l1_control.get_roll_setpoint(); - _fw_pos_ctrl_status.nav_pitch = get_tecs_pitch(); - _fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); - - _fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing(); - _fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error(); - - Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); - - _fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), - (double)curr_wp(0), (double)curr_wp(1)); - - fw_pos_ctrl_status_publish(); + // only publish status in full FW mode + if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + status_publish(); + } } } @@ -1826,10 +1900,9 @@ FixedwingPositionControl::reset_landing_state() _land_onslope = false; // reset abort land, unless loitering after an abort - if (_fw_pos_ctrl_status.abort_landing - && _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) { + if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) { - _fw_pos_ctrl_status.abort_landing = false; + abort_landing(false); } } @@ -1962,52 +2035,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - tecs_status_s t; - t.timestamp = _tecs.timestamp(); - - switch (_tecs.tecs_mode()) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; - } - - t.altitude_sp = _tecs.hgt_setpoint_adj(); - t.altitude_filtered = _tecs.vert_pos_state(); - t.airspeed_sp = _tecs.TAS_setpoint_adj(); - t.airspeed_filtered = _tecs.tas_state(); - - t.flight_path_angle_sp = _tecs.hgt_rate_setpoint(); - t.flight_path_angle = _tecs.vert_vel_state(); - - t.airspeed_derivative_sp = _tecs.TAS_rate_setpoint(); - t.airspeed_derivative = _tecs.speed_derivative(); - - t.total_energy_error = _tecs.STE_error(); - t.total_energy_rate_error = _tecs.STE_rate_error(); - t.energy_distribution_error = _tecs.SEB_error(); - t.energy_distribution_rate_error = _tecs.SEB_rate_error(); - - t.throttle_integ = _tecs.throttle_integ_state(); - t.pitch_integ = _tecs.pitch_integ_state(); - - if (_tecs_status_pub != nullptr) { - orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); - - } else { - _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); - } + tecs_status_publish(); } FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[]) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9cd2b8c749..cf1dc40c4d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -55,22 +55,23 @@ #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 @@ -86,23 +87,6 @@ #include #include -static constexpr float HDG_HOLD_DIST_NEXT = - 3000.0f; // initial distance of waypoint in front of plane in heading hold mode -static constexpr float HDG_HOLD_REACHED_DIST = - 1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode -static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane -static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode -static constexpr float HDG_HOLD_MAN_INPUT_THRESH = - 0.01f; // max manual roll/yaw input from user which does not change the locked heading - -static constexpr hrt_abstime T_ALT_TIMEOUT = 1; // time after which we abort landing if terrain estimate is not valid - -static constexpr float THROTTLE_THRESH = - 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = - 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode -static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; - using math::constrain; using math::max; using math::min; @@ -118,6 +102,24 @@ using uORB::Subscription; using namespace launchdetection; using namespace runwaytakeoff; +using namespace time_literals; + +static constexpr float HDG_HOLD_DIST_NEXT = + 3000.0f; // initial distance of waypoint in front of plane in heading hold mode +static constexpr float HDG_HOLD_REACHED_DIST = + 1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode +static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane +static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode +static constexpr float HDG_HOLD_MAN_INPUT_THRESH = + 0.01f; // max manual roll/yaw input from user which does not change the locked heading + +static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid + +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = + 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; class FixedwingPositionControl final : public ModuleBase, public ModuleParams { @@ -161,12 +163,12 @@ private: int _sensor_baro_sub{-1}; orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */ + orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */ + orb_advert_t _pos_ctrl_landing_status_pub{nullptr}; ///< landing status publication */ orb_advert_t _tecs_status_pub{nullptr}; ///< TECS status publication */ - orb_advert_t _fw_pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */ orb_id_t _attitude_setpoint_id{nullptr}; - fw_pos_ctrl_status_s _fw_pos_ctrl_status {}; ///< navigation capabilities */ 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 */ @@ -202,6 +204,7 @@ private: bool _land_stayonground{false}; bool _land_motor_lim{false}; bool _land_onslope{false}; + bool _land_abort{false}; Landingslope _landingslope; @@ -367,9 +370,7 @@ private: } _parameter_handles {}; ///< handles for interesting parameters */ - /** - * Update our local parameter cache. - */ + // Update our local parameter cache. int parameters_update(); // Update subscriptions @@ -383,8 +384,11 @@ private: void vehicle_land_detected_poll(); void vehicle_status_poll(); - // publish navigation capabilities - void fw_pos_ctrl_status_publish(); + void status_publish(); + void landing_status_publish(); + void tecs_status_publish(); + + void abort_landing(bool abort); /** * Get a new waypoint based on heading and distance from current position diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 23e03e4009..cf8991b90e 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -150,12 +150,6 @@ GroundRoverPositionControl::parameters_update() _parameters.speed_imax, _parameters.gndspeed_max); - /* Update and publish the navigation capabilities */ - _gnd_pos_ctrl_status.landing_slope_angle_rad = 0; - _gnd_pos_ctrl_status.landing_horizontal_slope_displacement = 0; - _gnd_pos_ctrl_status.landing_flare_length = 0; - gnd_pos_ctrl_status_publish(); - return OK; } @@ -192,18 +186,6 @@ GroundRoverPositionControl::position_setpoint_triplet_poll() } } -void GroundRoverPositionControl::gnd_pos_ctrl_status_publish() -{ - _gnd_pos_ctrl_status.timestamp = hrt_absolute_time(); - - if (_gnd_pos_ctrl_status_pub != nullptr) { - orb_publish(ORB_ID(fw_pos_ctrl_status), _gnd_pos_ctrl_status_pub, &_gnd_pos_ctrl_status); - - } else { - _gnd_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_gnd_pos_ctrl_status); - } -} - bool GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -440,31 +422,34 @@ GroundRoverPositionControl::task_main() /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } - } - /* XXX check if radius makes sense here */ - float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f); + /* XXX check if radius makes sense here */ + float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f); - /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_gnd_pos_ctrl_status.timestamp) > 1000000) - || (fabsf(turn_distance - _gnd_pos_ctrl_status.turn_distance) > FLT_EPSILON - && turn_distance > 0)) { + // publish status + position_controller_status_s pos_ctrl_status = {}; - /* set new turn distance */ - _gnd_pos_ctrl_status.turn_distance = turn_distance; + pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint(); + pos_ctrl_status.nav_pitch = 0.0f; + pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing(); - _gnd_pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint(); - _gnd_pos_ctrl_status.nav_pitch = 0.0f; - _gnd_pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing(); + pos_ctrl_status.target_bearing = _gnd_control.target_bearing(); + pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error(); - _gnd_pos_ctrl_status.target_bearing = _gnd_control.target_bearing(); - _gnd_pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error(); + pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - matrix::Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon); - _gnd_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint((double)current_position(0), (double)current_position(1), - (double)curr_wp(0), (double)curr_wp(1)); + pos_ctrl_status.acceptance_radius = turn_distance; + pos_ctrl_status.yaw_acceptance = NAN; - gnd_pos_ctrl_status_publish(); + pos_ctrl_status.timestamp = hrt_absolute_time(); + + if (_pos_ctrl_status_pub != nullptr) { + orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status); + + } else { + _pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status); + } } } diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp index cbbfdcc913..e400ad883f 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp @@ -41,23 +41,22 @@ * @author Marco Zorzi */ +#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 @@ -94,7 +93,7 @@ public: private: orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint */ - orb_advert_t _gnd_pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */ + orb_advert_t _pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */ bool _task_should_exit{false}; /**< if true, sensor task should exit */ bool _task_running{false}; /**< if true, task is running in its mainloop */ @@ -105,7 +104,6 @@ private: int _params_sub{-1}; /**< notification of parameter updates */ int _pos_sp_triplet_sub{-1}; - fw_pos_ctrl_status_s _gnd_pos_ctrl_status{}; /**< navigation capabilities */ manual_control_setpoint_s _manual{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */ @@ -188,11 +186,6 @@ private: void position_setpoint_triplet_poll(); void vehicle_control_mode_poll(); - /** - * Publish navigation capabilities - */ - void gnd_pos_ctrl_status_publish(); - /** * Control position. */ diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 1ceeebb366..a93bb3d098 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -655,10 +655,11 @@ void Logger::add_default_topics() add_topic("actuator_controls_virtual_fw"); add_topic("actuator_controls_virtual_mc"); add_topic("commander_state"); - add_topic("fw_pos_ctrl_status"); add_topic("fw_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint"); add_topic("multirotor_motor_limits"); + add_topic("position_controller_status"); + add_topic("position_controller_landingstatus"); add_topic("offboard_control_mode"); add_topic("time_offset"); add_topic("vehicle_attitude_groundtruth", 10); diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index bc7a8a5203..a60cc9e22f 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -39,17 +39,16 @@ #include "mavlink_high_latency2.h" -#include -#include -#include #include - +#include +#include +#include #include #include #include #include -#include #include +#include #include #include #include @@ -75,8 +74,8 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _battery_time(0), _estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), _estimator_status_time(0), - _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), - _fw_pos_ctrl_status_time(0), + _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))), + _pos_ctrl_status_time(0), _geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))), _geofence_time(0), _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), @@ -283,13 +282,13 @@ bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t * bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg) { - struct fw_pos_ctrl_status_s fw_pos_ctrl_status; + position_controller_status_s pos_ctrl_status = {}; - const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status); + const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status); - if (_fw_pos_ctrl_status_time > 0) { + if (_pos_ctrl_status_time > 0) { uint16_t target_distance; - convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, target_distance); + convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance); msg->target_distance = target_distance; } diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index a5ec6dbc73..052cb63c93 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -101,8 +101,8 @@ private: MavlinkOrbSubscription *_estimator_status_sub; uint64_t _estimator_status_time; - MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; - uint64_t _fw_pos_ctrl_status_time; + MavlinkOrbSubscription *_pos_ctrl_status_sub; + uint64_t _pos_ctrl_status_time; MavlinkOrbSubscription *_geofence_sub; uint64_t _geofence_time; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 898894c76d..9a2cc4a262 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -49,8 +49,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -69,13 +69,13 @@ #include #include #include -#include #include #include #include #include #include #include +#include #include #include #include @@ -3743,15 +3743,15 @@ public: unsigned get_size() { - return (_fw_pos_ctrl_status_sub->is_published()) ? + return (_pos_ctrl_status_sub->is_published()) ? MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; + MavlinkOrbSubscription *_pos_ctrl_status_sub; MavlinkOrbSubscription *_tecs_status_sub; - uint64_t _fw_pos_ctrl_status_timestamp{0}; + uint64_t _pos_ctrl_status_timestamp{0}; uint64_t _tecs_status_timestamp{0}; /* do not allow top copying this class */ @@ -3760,30 +3760,30 @@ private: protected: explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink), - _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), + _pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))), _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))) {} bool send(const hrt_abstime t) { - fw_pos_ctrl_status_s _fw_pos_ctrl_status = {}; - tecs_status_s _tecs_status = {}; + position_controller_status_s pos_ctrl_status = {}; + tecs_status_s tecs_status = {}; bool updated = false; - updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_timestamp, &_fw_pos_ctrl_status); - updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &_tecs_status); + updated |= _pos_ctrl_status_sub->update(&_pos_ctrl_status_timestamp, &pos_ctrl_status); + updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &tecs_status); if (updated) { mavlink_nav_controller_output_t msg = {}; - msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll); - msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch); - msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing); - msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing); - msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist; - msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error; - msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitude_sp; - msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeed_sp; + msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll); + msg.nav_pitch = math::degrees(pos_ctrl_status.nav_pitch); + msg.nav_bearing = (int16_t)math::degrees(pos_ctrl_status.nav_bearing); + msg.target_bearing = (int16_t)math::degrees(pos_ctrl_status.target_bearing); + msg.wp_dist = (uint16_t)pos_ctrl_status.wp_dist; + msg.xtrack_error = pos_ctrl_status.xtrack_error; + msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp; + msg.aspd_error = tecs_status.airspeed_filtered - tecs_status.airspeed_sp; mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index beaa1ef61a..dfa3baa29d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -47,8 +47,10 @@ #include #include #include -#include +#include #include +#include +#include bool MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, @@ -395,19 +397,22 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool if (MissionBlock::item_contains_position(missionitem_previous)) { - const bool fw_status_valid = (_navigator->get_fw_pos_ctrl_status()->timestamp > 0); + uORB::Subscription landing_status{ORB_ID(position_controller_landing_status)}; + landing_status.forcedUpdate(); + + const bool landing_status_valid = (landing_status.get().timestamp > 0); const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat, missionitem.lon); - if (fw_status_valid && (wp_distance > _navigator->get_fw_pos_ctrl_status()->landing_flare_length)) { + if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) { /* Last wp is before flare region */ const float delta_altitude = missionitem.altitude - missionitem_previous.altitude; if (delta_altitude < 0) { - const float horizontal_slope_displacement = _navigator->get_fw_pos_ctrl_status()->landing_horizontal_slope_displacement; - const float slope_angle_rad = _navigator->get_fw_pos_ctrl_status()->landing_slope_angle_rad; + const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement; + const float slope_angle_rad = landing_status.get().slope_angle_rad; const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, horizontal_slope_displacement, slope_angle_rad); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94f36000bf..d28e48cbba 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -43,7 +43,6 @@ #include #include -#include class Geofence; class Navigator; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f362c86922..2633522a94 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -55,15 +55,17 @@ #include "rtl.h" #include "takeoff.h" -#include -#include +#include "navigation.h" + +#include #include -#include -#include +#include +#include #include #include #include #include +#include #include #include #include @@ -145,7 +147,6 @@ public: /** * Getters */ - struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; } struct home_position_s *get_home_position() { return &_home_pos; } struct mission_result_s *get_mission_result() { return &_mission_result; } struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } @@ -278,7 +279,6 @@ public: bool force_vtol(); private: - int _fw_pos_ctrl_status_sub{-1}; /**< notification of vehicle capabilities updates */ int _global_pos_sub{-1}; /**< global position subscription */ int _gps_pos_sub{-1}; /**< gps position subscription */ int _home_pos_sub{-1}; /**< home position subscription */ @@ -286,6 +286,7 @@ private: int _local_pos_sub{-1}; /**< local position subscription */ int _offboard_mission_sub{-1}; /**< offboard mission subscription */ int _param_update_sub{-1}; /**< param update subscription */ + int _pos_ctrl_landing_status_sub{-1}; /**< position controller landing status subscription */ int _traffic_sub{-1}; /**< traffic subscription */ int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */ int _vstatus_sub{-1}; /**< vehicle status subscription */ @@ -299,7 +300,6 @@ private: orb_advert_t _vehicle_roi_pub{nullptr}; // Subscriptions - fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */ home_position_s _home_pos{}; /**< home position for RTL */ mission_result_s _mission_result{}; vehicle_global_position_s _global_pos{}; /**< global vehicle position */ @@ -307,7 +307,11 @@ private: vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ - uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/ + + uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; + + uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/ + // Publications geofence_result_s _geofence_result{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ @@ -369,7 +373,6 @@ private: float _mission_throttle{-1.0f}; // update subscriptions - void fw_pos_ctrl_status_update(bool force = false); void global_position_update(); void gps_position_update(); void home_position_update(bool force = false); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ec48a4c31b..190eed7239 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -46,26 +46,24 @@ #include "navigator.h" #include -#include #include -#include #include #include #include -#include +#include #include #include #include #include #include -#include #include #include +#include +#include #include #include #include -#include #include /** @@ -77,6 +75,8 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); #define GEOFENCE_CHECK_INTERVAL 200000 +using namespace time_literals; + namespace navigator { Navigator *g_navigator; @@ -141,17 +141,6 @@ Navigator::home_position_update(bool force) } } -void -Navigator::fw_pos_ctrl_status_update(bool force) -{ - bool updated = false; - orb_check(_fw_pos_ctrl_status_sub, &updated); - - if (updated || force) { - orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); - } -} - void Navigator::vehicle_status_update() { @@ -193,7 +182,7 @@ Navigator::run() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); + _pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); @@ -209,7 +198,6 @@ Navigator::run() local_position_update(); gps_position_update(); home_position_update(true); - fw_pos_ctrl_status_update(true); params_update(); /* wakeup source(s) */ @@ -293,11 +281,7 @@ Navigator::run() } /* navigation capabilities updated */ - orb_check(_fw_pos_ctrl_status_sub, &updated); - - if (updated) { - fw_pos_ctrl_status_update(); - } + _position_controller_status_sub.update(); /* home position updated */ orb_check(_home_pos_sub, &updated); @@ -762,14 +746,11 @@ Navigator::run() } if (_pos_sp_triplet_updated) { - _pos_sp_triplet.timestamp = hrt_absolute_time(); publish_position_setpoint_triplet(); - _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); - _mission_result_updated = false; } perf_end(_loop_perf); @@ -778,7 +759,7 @@ Navigator::run() orb_unsubscribe(_global_pos_sub); orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_gps_pos_sub); - orb_unsubscribe(_fw_pos_ctrl_status_sub); + orb_unsubscribe(_pos_ctrl_landing_status_sub); orb_unsubscribe(_vstatus_sub); orb_unsubscribe(_land_detected_sub); orb_unsubscribe(_home_pos_sub); @@ -827,11 +808,13 @@ Navigator::print_status() void Navigator::publish_position_setpoint_triplet() { - /* do not publish an empty triplet */ + // do not publish an invalid setpoint if (!_pos_sp_triplet.current.valid) { return; } + _pos_sp_triplet.timestamp = hrt_absolute_time(); + /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub != nullptr) { orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); @@ -839,6 +822,8 @@ Navigator::publish_position_setpoint_triplet() } else { _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } + + _pos_sp_triplet_updated = false; } float @@ -949,10 +934,11 @@ Navigator::get_acceptance_radius(float mission_item_radius) // when in fixed wing mode // this might need locking against a commanded transition // so that a stale _vstatus doesn't trigger an accepted mission item. - if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) { - if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) { - radius = _fw_pos_ctrl_status.turn_distance; - } + + const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + + if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && pos_ctrl_status.acceptance_radius > radius) { + radius = pos_ctrl_status.acceptance_radius; } return radius; @@ -1114,15 +1100,24 @@ void Navigator::check_traffic() bool Navigator::abort_landing() { + // only abort if currently landing and position controller status updated bool should_abort = false; - if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) { - if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) { + if (_pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - if (get_position_setpoint_triplet()->current.valid - && get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + bool updated = false; - should_abort = _fw_pos_ctrl_status.abort_landing; + orb_check(_pos_ctrl_landing_status_sub, &updated); + + if (updated) { + position_controller_landing_status_s landing_status = {}; + + // landing status from position controller must be newer than navigator's last position setpoint + if (orb_copy(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_sub, &landing_status) == PX4_OK) { + if (landing_status.timestamp > _pos_sp_triplet.timestamp) { + should_abort = landing_status.abort_landing; + } } } } @@ -1214,6 +1209,8 @@ Navigator::publish_mission_result() _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; + + _mission_result_updated = false; } void