mirror of https://github.com/ArduPilot/ardupilot
Working implementation of Jump Command
This commit is contained in:
parent
fc3e7767ba
commit
16eb5641c2
|
@ -691,7 +691,7 @@ static void medium_loop()
|
||||||
// --------------------
|
// --------------------
|
||||||
if(control_mode == AUTO){
|
if(control_mode == AUTO){
|
||||||
if(home_is_set == true && g.command_total > 1){
|
if(home_is_set == true && g.command_total > 1){
|
||||||
update_commands();
|
update_commands(true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -2,22 +2,10 @@
|
||||||
|
|
||||||
static void init_commands()
|
static void init_commands()
|
||||||
{
|
{
|
||||||
// Zero is home, the curren command
|
g.command_index = NO_COMMAND;
|
||||||
g.command_index = 0;
|
command_nav_index = NO_COMMAND;
|
||||||
|
command_cond_index = NO_COMMAND;
|
||||||
// This are registers for the current may and must commands
|
prev_nav_index = NO_COMMAND;
|
||||||
// setting to zero will allow them to be written to by new commands
|
|
||||||
command_nav_index = NO_COMMAND;
|
|
||||||
command_cond_index = NO_COMMAND;
|
|
||||||
prev_nav_index = NO_COMMAND;
|
|
||||||
|
|
||||||
// clear the command queue
|
|
||||||
clear_command_queue();
|
|
||||||
}
|
|
||||||
|
|
||||||
// forces the loading of a new command
|
|
||||||
// queue is emptied after a new command is processed
|
|
||||||
static void clear_command_queue(){
|
|
||||||
command_cond_queue.id = NO_COMMAND;
|
command_cond_queue.id = NO_COMMAND;
|
||||||
command_nav_queue.id = NO_COMMAND;
|
command_nav_queue.id = NO_COMMAND;
|
||||||
}
|
}
|
||||||
|
|
|
@ -660,25 +660,29 @@ static void do_loiter_at_location()
|
||||||
|
|
||||||
static void do_jump()
|
static void do_jump()
|
||||||
{
|
{
|
||||||
|
//Serial.printf("do Jump: %d\n", jump);
|
||||||
|
|
||||||
if(jump == -10){
|
if(jump == -10){
|
||||||
|
//Serial.printf("Fresh Jump\n");
|
||||||
|
// we use a locally stored index for jump
|
||||||
jump = command_cond_queue.lat;
|
jump = command_cond_queue.lat;
|
||||||
}
|
}
|
||||||
|
//Serial.printf("Jumps left: %d\n",jump);
|
||||||
|
|
||||||
if(jump > 0) {
|
if(jump > 0) {
|
||||||
|
//Serial.printf("Do Jump to %d\n",command_cond_queue.p1);
|
||||||
jump--;
|
jump--;
|
||||||
command_nav_index = 0;
|
change_command(command_cond_queue.p1);
|
||||||
command_cond_index = 0;
|
|
||||||
|
|
||||||
// set pointer to desired index
|
|
||||||
g.command_index = command_cond_queue.p1 - 1;
|
|
||||||
|
|
||||||
} else if (jump == 0){
|
} else if (jump == 0){
|
||||||
|
//Serial.printf("Did last jump\n");
|
||||||
// we're done, move along
|
// we're done, move along
|
||||||
jump = -10;
|
jump = -11;
|
||||||
|
|
||||||
} else if (jump == -1) {
|
} else if (jump == -1) {
|
||||||
|
//Serial.printf("jumpForever\n");
|
||||||
// repeat forever
|
// repeat forever
|
||||||
g.command_index = command_cond_queue.p1 - 1;
|
change_command(command_cond_queue.p1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -10,28 +10,24 @@ static void change_command(uint8_t cmd_index)
|
||||||
// load command
|
// load command
|
||||||
struct Location temp = get_cmd_with_index(cmd_index);
|
struct Location temp = get_cmd_with_index(cmd_index);
|
||||||
|
|
||||||
|
//Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
|
||||||
|
|
||||||
// verify it's a nav command
|
// verify it's a nav command
|
||||||
if (temp.id > MAV_CMD_NAV_LAST ){
|
if (temp.id > MAV_CMD_NAV_LAST ){
|
||||||
gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
|
//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
//Serial.printf("APM:New cmd Index: %d\n", cmd_index);
|
//Serial.printf("APM:New cmd Index: %d\n", cmd_index);
|
||||||
command_cond_index = NO_COMMAND;
|
init_commands();
|
||||||
command_cond_queue.id = NO_COMMAND;
|
command_nav_index = cmd_index;
|
||||||
|
|
||||||
command_nav_index = NO_COMMAND;
|
|
||||||
command_nav_queue.id = NO_COMMAND;
|
|
||||||
|
|
||||||
// we save one step back, because we add one in update
|
|
||||||
command_nav_index = cmd_index-1;
|
|
||||||
prev_nav_index = command_nav_index;
|
prev_nav_index = command_nav_index;
|
||||||
update_commands();
|
update_commands(false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// called by 10 Hz loop
|
// called by 10 Hz loop
|
||||||
// --------------------
|
// --------------------
|
||||||
static void update_commands(void)
|
static void update_commands(bool increment)
|
||||||
{
|
{
|
||||||
// A: if we do not have any commands there is nothing to do
|
// A: if we do not have any commands there is nothing to do
|
||||||
// B: We have completed the mission, don't redo the mission
|
// B: We have completed the mission, don't redo the mission
|
||||||
|
@ -45,7 +41,9 @@ static void update_commands(void)
|
||||||
if (command_nav_index < (g.command_total -1)) {
|
if (command_nav_index < (g.command_total -1)) {
|
||||||
|
|
||||||
// load next index
|
// load next index
|
||||||
command_nav_index++;
|
if (increment)
|
||||||
|
command_nav_index++;
|
||||||
|
|
||||||
command_nav_queue = get_cmd_with_index(command_nav_index);
|
command_nav_queue = get_cmd_with_index(command_nav_index);
|
||||||
|
|
||||||
if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){
|
if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){
|
||||||
|
|
Loading…
Reference in New Issue