fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW implement MAV_CMD_DO_GO_AROUND

Browse Source
This commit is contained in:
Daniel Agar
2016-08-06 20:56:53 +02:00
committed by Lorenz Meier
parent aa3ffa28c8
commit 2a15578f8d
9 changed files with 186 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+53
View File
@@ -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)
src/modules/navigator/mission.h
+6
View File
@@ -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;
src/modules/navigator/mission_block.cpp
+6 -3
View File
@@ -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;
}
src/modules/navigator/navigator.h
+5 -4
View File
@@ -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 */
src/modules/navigator/navigator_main.cpp
+24 -11
View File
@@ -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()
{