Copter: remove low-level handling of mission cmds
Now handled by AP_Mission
This commit is contained in:
Randy Mackay
parent
5d568502a0
commit
9f03e21f81
@ -1,106 +1,5 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
static void init_commands()
|
|
||||||
{
|
|
||||||
g.command_index = NO_COMMAND;
|
|
||||||
command_nav_index = NO_COMMAND;
|
|
||||||
command_cond_index = NO_COMMAND;
|
|
||||||
prev_nav_index = NO_COMMAND;
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
|
||||||
command_nav_queue.id = NO_COMMAND;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Getters
|
|
||||||
// -------
|
|
||||||
static struct Location get_cmd_with_index(int i)
|
|
||||||
{
|
|
||||||
struct Location temp;
|
|
||||||
|
|
||||||
// Find out proper location in memory by using the start_byte position + the index
|
|
||||||
// --------------------------------------------------------------------------------
|
|
||||||
if (i >= g.command_total) {
|
|
||||||
// we do not have a valid command to load
|
|
||||||
// return a WP with a "Blank" id
|
|
||||||
temp.id = CMD_BLANK;
|
|
||||||
|
|
||||||
// no reason to carry on
|
|
||||||
return temp;
|
|
||||||
|
|
||||||
}else{
|
|
||||||
// we can load a command, we don't process it yet
|
|
||||||
// read WP position
|
|
||||||
uint16_t mem = (WP_START_BYTE) + (i * WP_SIZE);
|
|
||||||
|
|
||||||
temp.id = hal.storage->read_byte(mem);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
temp.options = hal.storage->read_byte(mem);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
temp.p1 = hal.storage->read_byte(mem);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
temp.alt = hal.storage->read_dword(mem); // alt is stored in CM! Alt is stored relative!
|
|
||||||
|
|
||||||
mem += 4;
|
|
||||||
temp.lat = hal.storage->read_dword(mem); // lat is stored in decimal * 10,000,000
|
|
||||||
|
|
||||||
mem += 4;
|
|
||||||
temp.lng = hal.storage->read_dword(mem); // lon is stored in decimal * 10,000,000
|
|
||||||
}
|
|
||||||
|
|
||||||
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
|
|
||||||
//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && (temp.options & MASK_OPTIONS_RELATIVE_ALT)){
|
|
||||||
//temp.alt += home.alt;
|
|
||||||
//}
|
|
||||||
|
|
||||||
if(temp.options & WP_OPTION_RELATIVE) {
|
|
||||||
// If were relative, just offset from home
|
|
||||||
temp.lat += home.lat;
|
|
||||||
temp.lng += home.lng;
|
|
||||||
}
|
|
||||||
|
|
||||||
return temp;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Setters
|
|
||||||
// -------
|
|
||||||
static void set_cmd_with_index(struct Location temp, int i)
|
|
||||||
{
|
|
||||||
|
|
||||||
i = constrain_int16(i, 0, g.command_total.get());
|
|
||||||
|
|
||||||
// store home as 0 altitude!!!
|
|
||||||
// Home is always a MAV_CMD_NAV_WAYPOINT (16)
|
|
||||||
if (i == 0) {
|
|
||||||
temp.alt = 0;
|
|
||||||
temp.id = MAV_CMD_NAV_WAYPOINT;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
|
|
||||||
|
|
||||||
hal.storage->write_byte(mem, temp.id);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
hal.storage->write_byte(mem, temp.options);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
hal.storage->write_byte(mem, temp.p1);
|
|
||||||
|
|
||||||
mem++;
|
|
||||||
hal.storage->write_dword(mem, temp.alt); // Alt is stored in CM!
|
|
||||||
|
|
||||||
mem += 4;
|
|
||||||
hal.storage->write_dword(mem, temp.lat); // Lat is stored in decimal degrees * 10^7
|
|
||||||
|
|
||||||
mem += 4;
|
|
||||||
hal.storage->write_dword(mem, temp.lng); // Long is stored in decimal degrees * 10^7
|
|
||||||
|
|
||||||
// Make sure our WP_total
|
|
||||||
if(g.command_total < (i+1))
|
|
||||||
g.command_total.set_and_save(i+1);
|
|
||||||
}
|
|
||||||
|
|
||||||
static int32_t get_RTL_alt()
|
static int32_t get_RTL_alt()
|
||||||
{
|
{
|
||||||
if(g.rtl_altitude <= 0) {
|
if(g.rtl_altitude <= 0) {
|
||||||
@ -119,11 +18,6 @@ static void init_home()
|
|||||||
set_home_is_set(true);
|
set_home_is_set(true);
|
||||||
ahrs.set_home(g_gps->latitude, g_gps->longitude, 0);
|
ahrs.set_home(g_gps->latitude, g_gps->longitude, 0);
|
||||||
|
|
||||||
// Save Home to EEPROM
|
|
||||||
// -------------------
|
|
||||||
// no need to save this to EPROM
|
|
||||||
set_cmd_with_index(home, 0);
|
|
||||||
|
|
||||||
inertial_nav.setup_home_position();
|
inertial_nav.setup_home_position();
|
||||||
|
|
||||||
if (g.log_bitmask & MASK_LOG_CMD)
|
if (g.log_bitmask & MASK_LOG_CMD)
|
||||||
|
|||||||
@ -1,199 +0,0 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
||||||
|
|
||||||
// For changing active command mid-mission
|
|
||||||
//----------------------------------------
|
|
||||||
static void change_command(uint8_t cmd_index)
|
|
||||||
{
|
|
||||||
// check we are in AUTO mode
|
|
||||||
if (control_mode != AUTO) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// limit range
|
|
||||||
cmd_index = min(g.command_total - 1, cmd_index);
|
|
||||||
|
|
||||||
// load command
|
|
||||||
struct Location temp = get_cmd_with_index(cmd_index);
|
|
||||||
|
|
||||||
// verify it's a nav command
|
|
||||||
if(temp.id > MAV_CMD_NAV_LAST) {
|
|
||||||
|
|
||||||
}else{
|
|
||||||
// clear out command queue
|
|
||||||
init_commands();
|
|
||||||
|
|
||||||
// copy command to the queue
|
|
||||||
command_nav_queue = temp;
|
|
||||||
command_nav_index = cmd_index;
|
|
||||||
execute_nav_command();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// update_commands - initiates new navigation commands if we have completed the previous command
|
|
||||||
// called by 10 Hz loop
|
|
||||||
static void update_commands()
|
|
||||||
{
|
|
||||||
if(g.command_total <= 1)
|
|
||||||
return;
|
|
||||||
|
|
||||||
if(command_nav_queue.id == NO_COMMAND) {
|
|
||||||
// Our queue is empty
|
|
||||||
// fill command queue with a new command if available, or exit mission
|
|
||||||
// -------------------------------------------------------------------
|
|
||||||
|
|
||||||
// find next nav command
|
|
||||||
int16_t tmp_index;
|
|
||||||
|
|
||||||
if(command_nav_index < g.command_total) {
|
|
||||||
|
|
||||||
// what is the next index for a nav command?
|
|
||||||
tmp_index = find_next_nav_index(command_nav_index + 1);
|
|
||||||
|
|
||||||
if(tmp_index == -1) {
|
|
||||||
exit_mission();
|
|
||||||
return;
|
|
||||||
}else{
|
|
||||||
command_nav_index = tmp_index;
|
|
||||||
command_nav_queue = get_cmd_with_index(command_nav_index);
|
|
||||||
execute_nav_command();
|
|
||||||
}
|
|
||||||
}else{
|
|
||||||
// we are out of commands
|
|
||||||
exit_mission();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(command_cond_queue.id == NO_COMMAND) {
|
|
||||||
// Our queue is empty
|
|
||||||
// fill command queue with a new command if available, or do nothing
|
|
||||||
// -------------------------------------------------------------------
|
|
||||||
|
|
||||||
// no nav commands completed yet
|
|
||||||
if(prev_nav_index == NO_COMMAND)
|
|
||||||
return;
|
|
||||||
|
|
||||||
if(command_cond_index >= command_nav_index) {
|
|
||||||
// don't process the fututre
|
|
||||||
return;
|
|
||||||
|
|
||||||
}else if(command_cond_index == NO_COMMAND) {
|
|
||||||
// start from scratch
|
|
||||||
// look at command after the most recent completed nav
|
|
||||||
command_cond_index = prev_nav_index + 1;
|
|
||||||
|
|
||||||
}else{
|
|
||||||
// we've completed 1 cond, look at next command for another
|
|
||||||
command_cond_index++;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(command_cond_index < (g.command_total -2)) {
|
|
||||||
// we're OK to load a new command (last command must be a nav command)
|
|
||||||
command_cond_queue = get_cmd_with_index(command_cond_index);
|
|
||||||
|
|
||||||
if(command_cond_queue.id > MAV_CMD_CONDITION_LAST) {
|
|
||||||
// this is a do now command
|
|
||||||
process_now_command();
|
|
||||||
|
|
||||||
// clear command queue
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
|
||||||
|
|
||||||
}else if(command_cond_queue.id > MAV_CMD_NAV_LAST) {
|
|
||||||
// this is a conditional command
|
|
||||||
process_cond_command();
|
|
||||||
|
|
||||||
}else{
|
|
||||||
// this is a nav command, don't process
|
|
||||||
// clear the command conditional queue and index
|
|
||||||
prev_nav_index = NO_COMMAND;
|
|
||||||
command_cond_index = NO_COMMAND;
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// execute_nav_command - performs minor initialisation and logging before next navigation command in the queue is executed
|
|
||||||
static void execute_nav_command(void)
|
|
||||||
{
|
|
||||||
// This is what we report to MAVLINK
|
|
||||||
g.command_index = command_nav_index;
|
|
||||||
|
|
||||||
// Save CMD to Log
|
|
||||||
if(g.log_bitmask & MASK_LOG_CMD)
|
|
||||||
Log_Write_Cmd(g.command_index, &command_nav_queue);
|
|
||||||
|
|
||||||
// clear navigation prameters
|
|
||||||
reset_nav_params();
|
|
||||||
|
|
||||||
// Act on the new command
|
|
||||||
process_nav_command();
|
|
||||||
|
|
||||||
// clear May indexes to force loading of more commands
|
|
||||||
// existing May commands are tossed.
|
|
||||||
command_cond_index = NO_COMMAND;
|
|
||||||
}
|
|
||||||
|
|
||||||
// verify_commands - high level function to check if navigation and conditional commands have completed
|
|
||||||
static void verify_commands(void)
|
|
||||||
{
|
|
||||||
// check if navigation command completed
|
|
||||||
if(verify_nav_command()) {
|
|
||||||
// clear navigation command queue so next command can be loaded
|
|
||||||
command_nav_queue.id = NO_COMMAND;
|
|
||||||
|
|
||||||
// store our most recent executed nav command
|
|
||||||
prev_nav_index = command_nav_index;
|
|
||||||
|
|
||||||
// Wipe existing conditionals
|
|
||||||
command_cond_index = NO_COMMAND;
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check if conditional command completed
|
|
||||||
if(verify_cond_command()) {
|
|
||||||
// clear conditional command queue so next command can be loaded
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Finds the next navgation command in EEPROM
|
|
||||||
static int16_t find_next_nav_index(int16_t search_index)
|
|
||||||
{
|
|
||||||
Location tmp;
|
|
||||||
while(search_index < g.command_total) {
|
|
||||||
tmp = get_cmd_with_index(search_index);
|
|
||||||
if(tmp.id <= MAV_CMD_NAV_LAST) {
|
|
||||||
return search_index;
|
|
||||||
}else{
|
|
||||||
search_index++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void exit_mission()
|
|
||||||
{
|
|
||||||
// we are out of commands
|
|
||||||
g.command_index = 255;
|
|
||||||
|
|
||||||
// if we are not on the ground switch to loiter or land
|
|
||||||
if(!ap.land_complete) {
|
|
||||||
// try to enter loiter but if that fails land
|
|
||||||
if (!set_mode(LOITER)) {
|
|
||||||
set_mode(LAND);
|
|
||||||
}
|
|
||||||
}else{
|
|
||||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
|
||||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
|
||||||
if (g.rc_3.control_in == 0 || failsafe.radio) {
|
|
||||||
init_disarm_motors();
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
// if we've landed it's safe to disarm
|
|
||||||
init_disarm_motors();
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
@ -287,10 +287,6 @@ static void init_ardupilot()
|
|||||||
init_sonar();
|
init_sonar();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// initialize commands
|
|
||||||
// -------------------
|
|
||||||
init_commands();
|
|
||||||
|
|
||||||
// initialise the flight mode and aux switch
|
// initialise the flight mode and aux switch
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
reset_control_switch();
|
reset_control_switch();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user