fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 14:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into rtl_heading

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-18 18:19:12 +04:00
parent 36bd7a797b 3508a42f6a
commit b28b0d0fce
45 changed files with 1767 additions and 925 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence_params.c
+11 -5
View File
@@ -45,11 +45,17 @@
#include <systemlib/param/param.h>
/*
* geofence parameters, accessible via MAVLink
*
* Geofence parameters, accessible via MAVLink
*/
// @DisplayName Switch to enable geofence
// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
// @Range 0 or 1
/**
* Enable geofence.
*
* Set to 1 to enable geofence.
* Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ON, 1);
src/modules/navigator/navigator_main.cpp
+37 -34
View File
@@ -305,6 +305,12 @@ private:
void start_land();
void start_land_home();
/**
* Fork for state transitions
*/
void request_loiter_or_ready();
void request_mission_if_available();
/**
* Guards offboard mission
*/
@@ -699,24 +705,17 @@ Navigator::task_main()
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
request_loiter_or_ready();
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
request_mission_if_available();
stick_mode = true;
}
}
@@ -733,17 +732,11 @@ Navigator::task_main()
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
break;
case NAV_STATE_RTL:
@@ -770,12 +763,7 @@ Navigator::task_main()
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_mission_if_available();
}
}
}
@@ -1071,7 +1059,7 @@ Navigator::start_loiter()
float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
_pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
@@ -1080,9 +1068,8 @@ Navigator::start_loiter()
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
@@ -1405,6 +1392,28 @@ Navigator::set_rtl_item()
_pos_sp_triplet_updated = true;
}
void
Navigator::request_loiter_or_ready()
{
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
void
Navigator::request_mission_if_available()
{
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
request_loiter_or_ready();
}
}
void
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
{
@@ -1574,13 +1583,7 @@ Navigator::on_mission_item_reached()
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
if (_vstatus.condition_landed) {
dispatch(EVENT_READY_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
request_loiter_or_ready();
}
} else if (myState == NAV_STATE_RTL) {
src/modules/navigator/navigator_params.c
+82 -5
View File
@@ -50,14 +50,91 @@
/*
* Navigator parameters, accessible via MAVLink
*
*/
/**
* Minimum altitude (fixed wing only)
*
* Minimum altitude above home for LOITER.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
/**
* Waypoint acceptance radius
*
* Default value of acceptance radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
/**
* Loiter radius (fixed wing only)
*
* Default value of loiter radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Enable onboard mission
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Landing altitude
*
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
/**
* Return-To-Launch altitude
*
* Minimum altitude above home position for going home in RTL mode.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
/**
* Return-To-Launch delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
/**
* Enable parachute deployment
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);