From eae726e3456978a403b0ae26c9c2a2701153451f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Apr 2016 21:45:34 -0400 Subject: [PATCH] FW add mavlink NAV_CONTROLLER_OUTPUT --- ...apabilities.msg => fw_pos_ctrl_status.msg} | 9 ++ .../fw_pos_control_l1_main.cpp | 62 +++++---- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_messages.cpp | 121 ++++++++++++++---- .../navigator/mission_feasibility_checker.cpp | 15 +-- .../navigator/mission_feasibility_checker.h | 6 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 26 ++-- src/modules/uORB/objects_common.cpp | 4 +- 9 files changed, 170 insertions(+), 84 deletions(-) rename msg/{navigation_capabilities.msg => fw_pos_ctrl_status.msg} (68%) diff --git a/msg/navigation_capabilities.msg b/msg/fw_pos_ctrl_status.msg similarity index 68% rename from msg/navigation_capabilities.msg rename to msg/fw_pos_ctrl_status.msg index 6d12aaaed9..641b591205 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/fw_pos_ctrl_status.msg @@ -1,5 +1,14 @@ uint64 timestamp # timestamp this topic was emitted + +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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e93fba958..c510c4e322 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -86,9 +86,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -170,13 +170,13 @@ private: orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ - orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_advert_t _fw_pos_ctrl_status_pub; /**< navigation capabilities publication */ orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ @@ -408,7 +408,7 @@ private: /** * Publish navigation capabilities */ - void navigation_capabilities_publish(); + void fw_pos_ctrl_status_publish(); /** * Get a new waypoint based on heading and distance from current position @@ -530,7 +530,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(nullptr), _tecs_status_pub(nullptr), - _nav_capabilities_pub(nullptr), + _fw_pos_ctrl_status_pub(nullptr), /* publication ID */ _attitude_setpoint_id(0), @@ -538,7 +538,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* states */ _ctrl_state(), _att_sp(), - _nav_capabilities(), + _fw_pos_ctrl_status(), _manual(), _control_mode(), _vehicle_status(), @@ -593,7 +593,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _l1_control(), _control_mode_current(FW_POSCTRL_MODE_OTHER) { - _nav_capabilities = {}; + _fw_pos_ctrl_status = {}; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); @@ -769,10 +769,10 @@ FixedwingPositionControl::parameters_update() _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ - _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); - _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement(); - _nav_capabilities.landing_flare_length = landingslope.flare_length(); - navigation_capabilities_publish(); + _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(); /* Update Launch Detector Parameters */ launchDetector.updateParams(); @@ -1016,15 +1016,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c } } -void FixedwingPositionControl::navigation_capabilities_publish() +void FixedwingPositionControl::fw_pos_ctrl_status_publish() { - _nav_capabilities.timestamp = hrt_absolute_time(); + _fw_pos_ctrl_status.timestamp = hrt_absolute_time(); - if (_nav_capabilities_pub != nullptr) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + if (_fw_pos_ctrl_status_pub != nullptr) { + orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status); } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + _fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status); } } @@ -1337,7 +1337,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float alt_sp; - if (_nav_capabilities.abort_landing == true) { + if (_fw_pos_ctrl_status.abort_landing == true) { // if we entered loiter due to an aborted landing, demand // altitude setpoint well above landing waypoint alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; @@ -1349,7 +1349,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (in_takeoff_situation() || ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff) - && _nav_capabilities.abort_landing == true)) { + && _fw_pos_ctrl_status.abort_landing == true)) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); @@ -1465,7 +1465,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { // still no valid terrain, abort landing terrain_alt = pos_sp_triplet.current.alt; - _nav_capabilities.abort_landing = true; + _fw_pos_ctrl_status.abort_landing = true; } } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) @@ -1478,7 +1478,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { // terrain alt was not valid for long time, abort landing terrain_alt = _t_alt_prev_valid; - _nav_capabilities.abort_landing = true; + _fw_pos_ctrl_status.abort_landing = true; } } else { @@ -1954,7 +1954,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _time_last_t_alt = 0; // reset lading abort state - _nav_capabilities.abort_landing = false; + _fw_pos_ctrl_status.abort_landing = false; /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -2169,15 +2169,24 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) - || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + 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 */ - _nav_capabilities.turn_distance = turn_distance; + _fw_pos_ctrl_status.turn_distance = turn_distance; - navigation_capabilities_publish(); + _fw_pos_ctrl_status.nav_roll = _l1_control.nav_roll(); + _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(); + + math::Vector<2> 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(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + + fw_pos_ctrl_status_publish(); } } @@ -2242,9 +2251,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _was_in_transition = true; _asp_after_transition = _ctrl_state.airspeed; - // after transition we ramp up desired airspeed from the speed we had coming out of the transition - } else if (_was_in_transition) { + // after transition we ramp up desired airspeed from the speed we had coming out of the transition _asp_after_transition += dt * 2; // increase 2m/s if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 650fd3c34f..c522c4db6c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1840,6 +1840,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("ESTIMATOR_STATUS", 0.5f); configure_stream("ADSB_VEHICLE", 2.0f); + configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1870,6 +1871,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VISION_POSITION_NED", 10.0f); configure_stream("ESTIMATOR_STATUS", 1.0f); configure_stream("ADSB_VEHICLE", 10.0f); + configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); break; case MAVLINK_MODE_OSD: @@ -1919,6 +1921,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VISION_POSITION_NED", 10.0f); configure_stream("ESTIMATOR_STATUS", 5.0f); configure_stream("ADSB_VEHICLE", 20.0f); + configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f); default: break; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 39e47720f4..a480a4dcb7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -46,38 +46,39 @@ #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 -#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 @@ -2576,6 +2577,72 @@ protected: } }; +class MavlinkStreamNavControllerOutput : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNavControllerOutput::get_name_static(); + } + + static const char *get_name_static() + { + return "NAV_CONTROLLER_OUTPUT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNavControllerOutput(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; + MavlinkOrbSubscription *_tecs_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamNavControllerOutput(MavlinkStreamNavControllerOutput &); + MavlinkStreamNavControllerOutput& operator = (const MavlinkStreamNavControllerOutput &); + +protected: + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink), + _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), + _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))) + {} + + void send(const hrt_abstime t) + { + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {}; + struct tecs_status_s _tecs_status = {}; + + const bool updated_fw_pos_ctrl_status = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status); + const bool updated_tecs = _tecs_status_sub->update(&_tecs_status); + + if (updated_fw_pos_ctrl_status || updated_tecs) { + 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.altitudeSp; + msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp; + + _mavlink->send_message(MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, &msg); + } + } +}; class MavlinkStreamCameraCapture : public MavlinkStream { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 63c441d8b8..bb68624437 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -55,11 +55,11 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_log_pub(nullptr), - _capabilities_sub(-1), + _fw_pos_ctrl_status_sub(-1), _initDone(false), _dist_1wp_ok(false) { - _nav_caps = {0}; + _fw_pos_ctrl_status = {}; } @@ -275,7 +275,6 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); @@ -293,15 +292,15 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); - float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); - float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); float delta_altitude = missionitem.altitude - missionitem_previous.altitude; // warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", // wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); // warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", // _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); - if (wp_distance > _nav_caps.landing_flare_length) { + if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) { /* Last wp is before flare region */ if (delta_altitude < 0) { @@ -424,14 +423,14 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){ void MissionFeasibilityChecker::updateNavigationCapabilities() { - (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); } void MissionFeasibilityChecker::init() { if (!_initDone) { - _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _initDone = true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index b372cd3219..db19e60dc6 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include "geofence.h" @@ -54,8 +54,8 @@ class MissionFeasibilityChecker private: orb_advert_t *_mavlink_log_pub; - int _capabilities_sub; - struct navigation_capabilities_s _nav_caps; + int _fw_pos_ctrl_status_sub; + struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; bool _initDone; bool _dist_1wp_ok; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 61cb005893..2e6db02ab4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -207,7 +207,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _land_detected_sub; /**< vehicle land detected subscription */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -229,7 +229,7 @@ private: sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ - navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */ @@ -301,9 +301,9 @@ private: void home_position_update(bool force=false); /** - * Retreive navigation capabilities + * Retrieve fixed wing navigation capabilities */ - void navigation_capabilities_update(); + void fw_pos_ctrl_status_update(); /** * Retrieve vehicle status diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d5d0095d64..a383af5d38 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -70,7 +70,7 @@ #include #include #include -#include +#include #include #include @@ -108,7 +108,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _land_detected_sub(-1), - _capabilities_sub(-1), + _fw_pos_ctrl_status_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -126,7 +126,7 @@ Navigator::Navigator() : _sensor_combined{}, _home_pos{}, _mission_item{}, - _nav_caps{}, + _fw_pos_ctrl_status{}, _pos_sp_triplet{}, _reposition_triplet{}, _takeoff_triplet{}, @@ -231,9 +231,9 @@ Navigator::home_position_update(bool force) } void -Navigator::navigation_capabilities_update() +Navigator::fw_pos_ctrl_status_update() { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); } void @@ -298,7 +298,7 @@ Navigator::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -316,7 +316,7 @@ Navigator::task_main() gps_position_update(); sensor_combined_update(); home_position_update(true); - navigation_capabilities_update(); + fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ @@ -395,9 +395,9 @@ Navigator::task_main() } /* navigation capabilities updated */ - orb_check(_capabilities_sub, &updated); + orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { - navigation_capabilities_update(); + fw_pos_ctrl_status_update(); } /* home position updated */ @@ -524,7 +524,7 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - if (_nav_caps.abort_landing) { + if (_fw_pos_ctrl_status.abort_landing) { // pos controller aborted landing, requests loiter // above landing waypoint _navigation_mode = &_loiter; @@ -732,9 +732,9 @@ 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 && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { - if (_nav_caps.turn_distance > radius) { - radius = _nav_caps.turn_distance; + 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; } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4bb34227b3..fa381a15e3 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -229,8 +229,8 @@ ORB_DEFINE(test_motor, struct test_motor_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); -#include "topics/navigation_capabilities.h" -ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); +#include "topics/fw_pos_ctrl_status.h" +ORB_DEFINE(fw_pos_ctrl_status, struct fw_pos_ctrl_status_s); #include "topics/esc_status.h" ORB_DEFINE(esc_status, struct esc_status_s);