mirror of https://github.com/ArduPilot/ardupilot
Plane: Implementation of do_ and verify_ for MAV_CMD_NAV_LOITER_TO_ALT
This commit is contained in:
parent
3d3267d472
commit
607d9ca839
|
@ -661,6 +661,8 @@ static struct {
|
||||||
// A value used in condition commands (eg delay, change alt, etc.)
|
// A value used in condition commands (eg delay, change alt, etc.)
|
||||||
// For example in a change altitude command, it is the altitude to change to.
|
// For example in a change altitude command, it is the altitude to change to.
|
||||||
static int32_t condition_value;
|
static int32_t condition_value;
|
||||||
|
// Sometimes there is a second condition required:
|
||||||
|
static int32_t condition_value2;
|
||||||
// A starting value used to check the status of a conditional command.
|
// A starting value used to check the status of a conditional command.
|
||||||
// For example in a delay command the condition_start records that start time for the delay
|
// For example in a delay command the condition_start records that start time for the delay
|
||||||
static uint32_t condition_start;
|
static uint32_t condition_start;
|
||||||
|
|
|
@ -13,6 +13,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd);
|
||||||
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||||
static void do_set_home(const AP_Mission::Mission_Command& cmd);
|
static void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||||
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
|
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
|
||||||
|
static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
|
||||||
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||||
#if CAMERA == ENABLED
|
#if CAMERA == ENABLED
|
||||||
static void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
static void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||||
|
@ -79,6 +80,10 @@ start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
do_loiter_time(cmd);
|
do_loiter_time(cmd);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TO_ALT:
|
||||||
|
do_loiter_to_alt(cmd);
|
||||||
|
break;
|
||||||
|
|
||||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||||
set_mode(RTL);
|
set_mode(RTL);
|
||||||
break;
|
break;
|
||||||
|
@ -222,6 +227,9 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
|
||||||
case MAV_CMD_NAV_LOITER_TIME:
|
case MAV_CMD_NAV_LOITER_TIME:
|
||||||
return verify_loiter_time();
|
return verify_loiter_time();
|
||||||
|
|
||||||
|
case MAV_CMD_NAV_LOITER_TO_ALT:
|
||||||
|
return verify_loiter_to_alt();
|
||||||
|
|
||||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||||
return verify_RTL();
|
return verify_RTL();
|
||||||
|
|
||||||
|
@ -366,6 +374,32 @@ static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||||
reset_offset_altitude();
|
reset_offset_altitude();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
|
||||||
|
{
|
||||||
|
//set target alt
|
||||||
|
next_WP_loc.alt = cmd.content.location.alt;
|
||||||
|
|
||||||
|
// convert relative alt to absolute alt
|
||||||
|
if (cmd.content.location.flags.relative_alt) {
|
||||||
|
next_WP_loc.flags.relative_alt = false;
|
||||||
|
next_WP_loc.alt += home.alt;
|
||||||
|
}
|
||||||
|
|
||||||
|
//I know I'm storing this twice -- I'm doing that on purpose --
|
||||||
|
//see verify_loiter_alt() function
|
||||||
|
condition_value = next_WP_loc.alt;
|
||||||
|
|
||||||
|
//are lat and lon 0? if so, don't change the current wp lat/lon
|
||||||
|
if (cmd.content.location.lat != 0 && cmd.content.location.lng != 0) {
|
||||||
|
set_next_WP(cmd.content.location);
|
||||||
|
}
|
||||||
|
//set loiter direction
|
||||||
|
loiter_set_direction_wp(cmd);
|
||||||
|
|
||||||
|
//must the plane be heading towards the next waypoint before breaking?
|
||||||
|
condition_value2 = LOWBYTE(cmd.p1);
|
||||||
|
}
|
||||||
|
|
||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
// Verify Nav (Must) commands
|
// Verify Nav (Must) commands
|
||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
@ -512,6 +546,67 @@ static bool verify_loiter_turns()
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool verify_loiter_to_alt() {
|
||||||
|
update_loiter();
|
||||||
|
|
||||||
|
//has target altitude been reached?
|
||||||
|
if (condition_value != 0) {
|
||||||
|
if (fabs(condition_value - current_loc.alt) < 500) {
|
||||||
|
//Only have to reach the altitude once -- that's why I need
|
||||||
|
//this global condition variable.
|
||||||
|
//
|
||||||
|
//This is in case of altitude oscillation when still trying
|
||||||
|
//to reach the target heading.
|
||||||
|
condition_value = 0;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//has target heading been reached?
|
||||||
|
if (condition_value2 != 0) {
|
||||||
|
//Get the lat/lon of next Nav waypoint after this one:
|
||||||
|
AP_Mission::Mission_Command next_nav_cmd;
|
||||||
|
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
|
||||||
|
next_nav_cmd)) {
|
||||||
|
//no next waypoint to shoot for -- go ahead and break out of loiter
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Bearing in radians
|
||||||
|
float bearing = (radians( (float)(get_bearing_cd(current_loc,next_nav_cmd.content.location)/100.0) ));
|
||||||
|
|
||||||
|
// Calculate heading
|
||||||
|
float heading = 5000;
|
||||||
|
if (((Compass &) compass).read()) {
|
||||||
|
const Matrix3f &m = ahrs.get_dcm_matrix();
|
||||||
|
heading = compass.calculate_heading(m);
|
||||||
|
|
||||||
|
//map heading to bearing's coordinate space:
|
||||||
|
if (heading < 0.0f) {
|
||||||
|
heading += 2.0f*PI;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check to see if the the plane is heading toward the land waypoint
|
||||||
|
//3 degrees = 0.0523598776 radians
|
||||||
|
if (fabs(bearing - heading) <= 0.0523598776f) {
|
||||||
|
//Want to head in a straight line from _here_ to the next waypoint.
|
||||||
|
//DON'T want to head in a line from the center of the loiter to
|
||||||
|
//the next waypoint.
|
||||||
|
//Therefore: mark the "last" (next_wp_loc is about to be updated)
|
||||||
|
//wp lat/lon as the current location.
|
||||||
|
next_WP_loc = current_loc;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
static bool verify_RTL()
|
static bool verify_RTL()
|
||||||
{
|
{
|
||||||
update_loiter();
|
update_loiter();
|
||||||
|
|
Loading…
Reference in New Issue