From 2a15578f8da18bfa862cfb4585e82adbef809b3c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 29 Jul 2016 20:24:21 -0400 Subject: [PATCH] FW implement MAV_CMD_DO_GO_AROUND --- Makefile | 2 +- msg/vehicle_command.msg | 3 +- src/modules/commander/commander.cpp | 1 + .../fw_pos_control_l1_main.cpp | 117 +++++++++++++----- src/modules/navigator/mission.cpp | 53 ++++++++ src/modules/navigator/mission.h | 6 + src/modules/navigator/mission_block.cpp | 9 +- src/modules/navigator/navigator.h | 9 +- src/modules/navigator/navigator_main.cpp | 35 ++++-- 9 files changed, 186 insertions(+), 49 deletions(-) diff --git a/Makefile b/Makefile index bbc62cb10d..e0349ef694 100644 --- a/Makefile +++ b/Makefile @@ -279,7 +279,7 @@ checks_last: \ check_format \ check: checks_defaults checks_tests checks_alts checks_uavcan checks_bootloaders checks_last -quick_check: check_px4fmu-v2_default check_px4fmu-v4_default check_tests check_format +quick_check: check_px4fmu-v2_default check_px4fmu-v4_default check_posix_sitl_default check_tests check_format check_format: $(call colorecho,"Checking formatting with astyle") diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index f310a0181f..2291538044 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -31,7 +31,8 @@ uint32 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay num uint32 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| -uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION=185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ uint32 VEHICLE_CMD_DO_REPOSITION = 192 uint32 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 uint32 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6e2eafa0a3..95249e50cd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1084,6 +1084,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: + case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: /* ignore commands that handled in low prio loop */ break; 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 d956bfcdfd..3fc327a33b 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 @@ -94,6 +94,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,7 @@ private: int _pos_sp_triplet_sub; int _ctrl_state_sub; /**< control state subscription */ int _control_mode_sub; /**< control mode subscription */ + int _vehicle_command_sub; /**< vehicle command subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _vehicle_land_detected_sub; /**< vehicle land detected subscription */ int _params_sub; /**< notification of parameter updates */ @@ -181,6 +183,7 @@ private: 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_command_s _vehicle_command; /**< vehicle commands */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ @@ -387,6 +390,11 @@ private: */ void vehicle_control_mode_poll(); + /** + * Check for new in vehicle commands + */ + void vehicle_command_poll(); + /** * Check for changes in vehicle status. */ @@ -477,6 +485,11 @@ private: void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + /** + * Handle incoming vehicle commands + */ + void handle_command(); + /** * Shim for calling task_main from task_create. */ @@ -533,6 +546,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet_sub(-1), _ctrl_state_sub(-1), _control_mode_sub(-1), + _vehicle_command_sub(-1), _vehicle_status_sub(-1), _vehicle_land_detected_sub(-1), _params_sub(-1), @@ -553,6 +567,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _fw_pos_ctrl_status(), _manual(), _control_mode(), + _vehicle_command(), _vehicle_status(), _vehicle_land_detected(), _global_pos(), @@ -830,6 +845,19 @@ 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); + handle_command(); + } +} + void FixedwingPositionControl::vehicle_status_poll() { @@ -1396,34 +1424,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - float alt_sp; - - 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; - - } else { - // use altitude given by waypoint - alt_sp = pos_sp_triplet.current.alt; - } + float alt_sp = pos_sp_triplet.current.alt; if (in_takeoff_situation()) { alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-5.0f), math::radians(5.0f)); } - if (in_takeoff_situation() || - ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff) - && _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(-10.0f), - math::radians(10.0f)); + if (_fw_pos_ctrl_status.abort_landing) { + if (pos_sp_triplet.current.alt - _global_pos.alt < _parameters.climbout_diff) { + // aborted landing complete, normal loiter over landing point + _fw_pos_ctrl_status.abort_landing = false; + + } else { + // continue straight until vehicle has sufficient altitude + _att_sp.roll_body = 0.0f; + } } - tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + tecs_update_pitch_throttle(alt_sp, + calculate_target_airspeed(mission_airspeed), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { @@ -1431,9 +1461,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // if they have been enabled using the corresponding parameter _att_sp.apply_flaps = true; - // save time at which we started landing + // save time at which we started landing and reset abort_landing if (_time_started_landing == 0) { _time_started_landing = hrt_absolute_time(); + + _fw_pos_ctrl_status.abort_landing = false; } float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); @@ -2037,13 +2069,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // reset hold altitude _hold_alt = _global_pos.alt; - // reset terrain estimation relevant values - _time_started_landing = 0; - _time_last_t_alt = 0; - - // reset lading abort state - _fw_pos_ctrl_status.abort_landing = false; - /* reset landing and takeoff state */ if (!_last_manual) { reset_landing_state(); @@ -2147,6 +2172,25 @@ FixedwingPositionControl::get_tecs_thrust() } } +void +FixedwingPositionControl::handle_command() +{ + if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { + // only abort landing before point of no return (horizontal and vertical) + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + + if (_land_noreturn_vertical) { + mavlink_log_info(&_mavlink_log_pub, "#Landing, can't abort after flare"); + + } else { + _fw_pos_ctrl_status.abort_landing = true; + mavlink_log_info(&_mavlink_log_pub, "#Landing, aborted"); + } + } + } +} + + void FixedwingPositionControl::task_main() { @@ -2159,6 +2203,7 @@ FixedwingPositionControl::task_main() _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _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)); @@ -2210,6 +2255,9 @@ FixedwingPositionControl::task_main() /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); + /* check for new vehicle commands */ + vehicle_command_poll(); + /* check vehicle status for changes to publication state */ vehicle_status_poll(); @@ -2321,12 +2369,23 @@ void FixedwingPositionControl::reset_landing_state() { _time_started_landing = 0; + // reset terrain estimation relevant values + _time_last_t_alt = 0; + _land_noreturn_horizontal = false; _land_noreturn_vertical = false; _land_stayonground = false; _land_motor_lim = false; _land_onslope = false; _land_useterrain = false; + + // reset abort land, unless loitering after an abort + if (_fw_pos_ctrl_status.abort_landing == true + && _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) { + + _fw_pos_ctrl_status.abort_landing = false; + } + } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f8729595d3..a671918c2e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -74,6 +74,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_altmode(this, "MIS_ALTMODE", false), _param_yawmode(this, "MIS_YAWMODE", false), _param_force_vtol(this, "VT_NAV_FORCE_VT", false), + _param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false), _onboard_mission{}, _offboard_mission{}, _current_onboard_mission_index(-1), @@ -226,9 +227,17 @@ Mission::on_active() && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) || _navigator->get_vstatus()->is_vtol) { + heading_sp_update(); } + /* check if landing needs to be aborted */ + if ((_mission_item.nav_cmd == NAV_CMD_LAND) + && (_navigator->abort_landing())) { + + do_abort_landing(); + } + } void @@ -382,6 +391,7 @@ Mission::set_mission_items() /* try setting onboard mission item */ if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) { + /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission"); @@ -819,6 +829,7 @@ Mission::heading_sp_update() || _mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND || _work_item_type == WORK_ITEM_TYPE_ALIGN) { + return; } @@ -852,6 +863,7 @@ Mission::heading_sp_update() // (which would result in a wrong yaw setpoint spike during back transition) && _navigator->get_vstatus()->is_rotary_wing && !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) { + point_to_latlon[0] = _navigator->get_home_position()->lat; point_to_latlon[1] = _navigator->get_home_position()->lon; @@ -957,6 +969,47 @@ Mission::altitude_sp_foh_reset() _min_current_sp_distance_xy = FLT_MAX; } +void +Mission::do_abort_landing() +{ + // Abort FW landing + // turn the land waypoint into a loiter and stay there + + if (_mission_item.nav_cmd != NAV_CMD_LAND) { + return; + } + + // loiter at the larger of MIS_LTRMIN_ALT above the landing point + // or 2 * FW_CLMBOUT_DIFF above the current altitude + float alt_landing = get_absolute_altitude_for_item(_mission_item); + float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), + _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get())); + + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = alt_sp; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + _navigator->set_position_setpoint_triplet_updated(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)", + (int)(alt_sp - alt_landing)); + + // move mission index back 1 (landing approach point) so that re-entering + // the mission doesn't try to land from the loiter above land + // TODO: reset index to MAV_CMD_DO_LAND_START + _current_offboard_mission_index -= 1; +} + bool Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item, struct mission_item_s *next_position_mission_item, bool *has_next_position_item) diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 890729459d..57e137dd9b 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -164,6 +164,11 @@ private: */ void altitude_sp_foh_reset(); + /** + * Abort landing + */ + void do_abort_landing(); + float get_absolute_altitude_for_item(struct mission_item_s &mission_item); /** @@ -230,6 +235,7 @@ private: control::BlockParamInt _param_altmode; control::BlockParamInt _param_yawmode; control::BlockParamInt _param_force_vtol; + control::BlockParamFloat _param_fw_climbout_diff; struct mission_s _onboard_mission; struct mission_s _offboard_mission; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1f840f2930..bb81641c1b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -276,14 +276,16 @@ MissionBlock::is_mission_item_reached() /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) - || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { + || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { + _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && - _param_yaw_timeout.get() >= FLT_EPSILON && - now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f) { + (_param_yaw_timeout.get() >= FLT_EPSILON) && + (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { + _navigator->set_mission_failure("unable to reach heading within timeout"); } @@ -403,6 +405,7 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { + return false; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7e793b99d9..7a80cf3e74 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -115,13 +115,13 @@ public: /** * Publish the geofence result */ - void publish_geofence_result(); + void publish_geofence_result(); /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure */ - void publish_att_sp(); + void publish_att_sp(); /** * Setters @@ -157,7 +157,7 @@ public: /** * Returns the default acceptance radius defined by the parameter */ - float get_default_acceptance_radius(); + float get_default_acceptance_radius(); /** * Get the acceptance radius @@ -185,7 +185,6 @@ public: */ void set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; } - /** * Get the target throttle * @@ -214,6 +213,8 @@ public: bool is_planned_mission() { return _navigation_mode == &_mission; } + bool abort_landing(); + private: bool _task_should_exit; /**< if true, sensor task should exit */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6804fe1be0..35ff95db3b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -540,15 +540,8 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - if (_fw_pos_ctrl_status.abort_landing) { - // pos controller aborted landing, requests loiter - // above landing waypoint - _navigation_mode = &_loiter; - _pos_sp_triplet_published_invalid_once = false; - } else { - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; - } + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; @@ -780,16 +773,36 @@ Navigator::get_acceptance_radius(float mission_item_radius) return radius; } -void Navigator::add_fence_point(int argc, char *argv[]) +void +Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); } -void Navigator::load_fence_from_file(const char *filename) +void +Navigator::load_fence_from_file(const char *filename) { _geofence.loadFromFile(filename); } +bool +Navigator::abort_landing() +{ + bool should_abort = false; + + if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) { + if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) { + + if (get_position_setpoint_triplet()->current.valid + && get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + + should_abort = _fw_pos_ctrl_status.abort_landing; + } + } + } + + return should_abort; +} static void usage() {