mirror of https://github.com/ArduPilot/ardupilot
Sub: Implement DO_NAV_LAND commands.
Auto return to surface in analagous manner to Copter's auto land.
This commit is contained in:
parent
fb9499fc6d
commit
9f5605ff57
|
@ -312,7 +312,6 @@ private:
|
||||||
int32_t home_distance;
|
int32_t home_distance;
|
||||||
// distance between plane and next waypoint in cm.
|
// distance between plane and next waypoint in cm.
|
||||||
uint32_t wp_distance;
|
uint32_t wp_distance;
|
||||||
uint8_t land_state; // records state of land (flying to location, descending)
|
|
||||||
|
|
||||||
// Auto
|
// Auto
|
||||||
AutoMode auto_mode; // controls which auto controller is run
|
AutoMode auto_mode; // controls which auto controller is run
|
||||||
|
@ -605,9 +604,6 @@ private:
|
||||||
void auto_wp_start(const Location_Class& dest_loc);
|
void auto_wp_start(const Location_Class& dest_loc);
|
||||||
void auto_wp_run();
|
void auto_wp_run();
|
||||||
void auto_spline_run();
|
void auto_spline_run();
|
||||||
void auto_land_start();
|
|
||||||
void auto_land_start(const Vector3f& destination);
|
|
||||||
void auto_land_run();
|
|
||||||
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
|
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
|
||||||
void auto_circle_start();
|
void auto_circle_start();
|
||||||
void auto_circle_run();
|
void auto_circle_run();
|
||||||
|
@ -789,7 +785,7 @@ private:
|
||||||
|
|
||||||
bool do_guided(const AP_Mission::Mission_Command& cmd);
|
bool do_guided(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_land(const AP_Mission::Mission_Command& cmd);
|
void do_surface(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_circle(const AP_Mission::Mission_Command& cmd);
|
void do_circle(const AP_Mission::Mission_Command& cmd);
|
||||||
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||||
|
@ -814,6 +810,7 @@ private:
|
||||||
void do_gripper(const AP_Mission::Mission_Command& cmd);
|
void do_gripper(const AP_Mission::Mission_Command& cmd);
|
||||||
#endif
|
#endif
|
||||||
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||||
|
bool verify_surface(const AP_Mission::Mission_Command& cmd);
|
||||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||||
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
|
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED == ENABLED
|
||||||
|
|
|
@ -1,5 +1,7 @@
|
||||||
#include "Sub.h"
|
#include "Sub.h"
|
||||||
|
|
||||||
|
static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
|
||||||
|
|
||||||
// start_command - this function will be called when the ap_mission lib wishes to start a new command
|
// start_command - this function will be called when the ap_mission lib wishes to start a new command
|
||||||
bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
|
@ -18,7 +20,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
|
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
|
||||||
do_land(cmd);
|
do_surface(cmd);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
|
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
|
||||||
|
@ -174,6 +176,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||||
case MAV_CMD_NAV_WAYPOINT:
|
case MAV_CMD_NAV_WAYPOINT:
|
||||||
return verify_nav_wp(cmd);
|
return verify_nav_wp(cmd);
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LAND:
|
||||||
|
return verify_surface(cmd);
|
||||||
|
|
||||||
case MAV_CMD_NAV_LOITER_UNLIM:
|
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||||
return verify_loiter_unlimited();
|
return verify_loiter_unlimited();
|
||||||
|
|
||||||
|
@ -282,39 +287,40 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// do_land - initiate landing procedure
|
// do_surface - initiate surface procedure
|
||||||
void Sub::do_land(const AP_Mission::Mission_Command& cmd)
|
void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
// To-Do: check if we have already landed
|
Location_Class target_location;
|
||||||
|
|
||||||
// if location provided we fly to that location at current altitude
|
// if location provided we fly to that location at current altitude
|
||||||
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
|
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
|
||||||
// set state to fly to location
|
// set state to go to location
|
||||||
land_state = LAND_STATE_FLY_TO_LOCATION;
|
auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
|
||||||
|
|
||||||
// calculate and set desired location above landing target
|
// calculate and set desired location below surface target
|
||||||
// convert to location class
|
// convert to location class
|
||||||
Location_Class target_loc(cmd.content.location);
|
target_location = Location_Class(cmd.content.location);
|
||||||
|
|
||||||
// decide if we will use terrain following
|
// decide if we will use terrain following
|
||||||
int32_t curr_terr_alt_cm, target_terr_alt_cm;
|
int32_t curr_terr_alt_cm, target_terr_alt_cm;
|
||||||
if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
|
if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
|
||||||
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
|
target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
|
||||||
curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
|
|
||||||
// if using terrain, set target altitude to current altitude above terrain
|
// if using terrain, set target altitude to current altitude above terrain
|
||||||
target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
|
target_location.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
|
||||||
} else {
|
} else {
|
||||||
// set target altitude to current altitude above home
|
// set target altitude to current altitude above home
|
||||||
target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
|
target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||||
}
|
}
|
||||||
auto_wp_start(target_loc);
|
|
||||||
} else {
|
} else {
|
||||||
// set landing state
|
// set surface state to ascend
|
||||||
land_state = LAND_STATE_DESCENDING;
|
auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
|
||||||
|
|
||||||
// initialise landing controller
|
// Set waypoint destination to current location at zero depth
|
||||||
auto_land_start();
|
target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Go to wp location
|
||||||
|
auto_wp_start(target_location);
|
||||||
}
|
}
|
||||||
|
|
||||||
// do_loiter_unlimited - start loitering with no end conditions
|
// do_loiter_unlimited - start loitering with no end conditions
|
||||||
|
@ -563,6 +569,44 @@ bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// verify_land - returns true if landing has been completed
|
||||||
|
bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
|
||||||
|
{
|
||||||
|
bool retval = false;
|
||||||
|
|
||||||
|
switch (auto_surface_state) {
|
||||||
|
case AUTO_SURFACE_STATE_GO_TO_LOCATION:
|
||||||
|
// check if we've reached the location
|
||||||
|
if (wp_nav.reached_wp_destination()) {
|
||||||
|
// Set target to current xy and zero depth
|
||||||
|
// TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
|
||||||
|
Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||||
|
|
||||||
|
auto_wp_start(target_location);
|
||||||
|
|
||||||
|
// advance to next state
|
||||||
|
auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AUTO_SURFACE_STATE_ASCEND:
|
||||||
|
// rely on THROTTLE_LAND mode to correctly update landing status
|
||||||
|
if (wp_nav.reached_wp_destination()) {
|
||||||
|
retval = true;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
// this should never happen
|
||||||
|
// TO-DO: log an error
|
||||||
|
retval = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// true is returned if we've successfully surfaced
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
bool Sub::verify_loiter_unlimited()
|
bool Sub::verify_loiter_unlimited()
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -56,10 +56,6 @@ void Sub::auto_run()
|
||||||
auto_wp_run();
|
auto_wp_run();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case Auto_Land:
|
|
||||||
auto_land_run();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case Auto_Circle:
|
case Auto_Circle:
|
||||||
auto_circle_run();
|
auto_circle_run();
|
||||||
break;
|
break;
|
||||||
|
@ -249,56 +245,6 @@ void Sub::auto_spline_run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// auto_land_start - initialises controller to implement a landing
|
|
||||||
void Sub::auto_land_start()
|
|
||||||
{
|
|
||||||
// set target to stopping point
|
|
||||||
Vector3f stopping_point;
|
|
||||||
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
|
||||||
|
|
||||||
// call location specific land start function
|
|
||||||
auto_land_start(stopping_point);
|
|
||||||
}
|
|
||||||
|
|
||||||
// auto_land_start - initialises controller to implement a landing
|
|
||||||
void Sub::auto_land_start(const Vector3f& destination)
|
|
||||||
{
|
|
||||||
auto_mode = Auto_Land;
|
|
||||||
|
|
||||||
// initialise loiter target destination
|
|
||||||
wp_nav.init_loiter_target(destination);
|
|
||||||
|
|
||||||
// initialise position and desired velocity
|
|
||||||
pos_control.set_alt_target(inertial_nav.get_altitude());
|
|
||||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
|
||||||
|
|
||||||
// initialise yaw
|
|
||||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
|
||||||
}
|
|
||||||
|
|
||||||
// auto_land_run - lands in auto mode
|
|
||||||
// called by auto_run at 100hz or more
|
|
||||||
void Sub::auto_land_run()
|
|
||||||
{
|
|
||||||
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
|
||||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
|
||||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
|
||||||
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
|
|
||||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
||||||
|
|
||||||
// set target to current position
|
|
||||||
wp_nav.init_loiter_target();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// set motors to full range
|
|
||||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
|
||||||
|
|
||||||
// land mode replaced by surface mode, does not have this functionality
|
|
||||||
// land_run_horizontal_control();
|
|
||||||
// land_run_vertical_control();
|
|
||||||
}
|
|
||||||
|
|
||||||
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
|
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
|
||||||
// we assume the caller has set the circle's circle with circle_nav.set_center()
|
// we assume the caller has set the circle's circle with circle_nav.set_center()
|
||||||
// we assume the caller has performed all required GPS_ok checks
|
// we assume the caller has performed all required GPS_ok checks
|
||||||
|
|
|
@ -13,6 +13,11 @@
|
||||||
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
|
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
|
||||||
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
|
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
|
||||||
|
|
||||||
|
enum AutoSurfaceState {
|
||||||
|
AUTO_SURFACE_STATE_GO_TO_LOCATION,
|
||||||
|
AUTO_SURFACE_STATE_ASCEND
|
||||||
|
};
|
||||||
|
|
||||||
// Autopilot Yaw Mode enumeration
|
// Autopilot Yaw Mode enumeration
|
||||||
enum autopilot_yaw_mode {
|
enum autopilot_yaw_mode {
|
||||||
AUTO_YAW_HOLD = 0, // pilot controls the heading
|
AUTO_YAW_HOLD = 0, // pilot controls the heading
|
||||||
|
@ -166,10 +171,6 @@ enum RTLState {
|
||||||
RTL_Land
|
RTL_Land
|
||||||
};
|
};
|
||||||
|
|
||||||
// LAND state
|
|
||||||
#define LAND_STATE_FLY_TO_LOCATION 0
|
|
||||||
#define LAND_STATE_DESCENDING 1
|
|
||||||
|
|
||||||
// Logging parameters
|
// Logging parameters
|
||||||
#define TYPE_AIRSTART_MSG 0x00
|
#define TYPE_AIRSTART_MSG 0x00
|
||||||
#define TYPE_GROUNDSTART_MSG 0x01
|
#define TYPE_GROUNDSTART_MSG 0x01
|
||||||
|
|
Loading…
Reference in New Issue