Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mission: support do-jump num_times is -1

Browse Source 
do-jump loops forever when num times is set to -1
This commit is contained in:
Randy Mackay 2014-02-28 21:23:38 +09:00
parent c6c43847b6
commit 85b979ede9
2 changed files with 38 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_Mission/AP_Mission.cpp
View File

@ -205,8 +205,8 @@ bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) cons
{
uint16_t pos_in_storage; // position in storage from where we will read the next byte
// exit immediately if index is beyond last command
if (index > _cmd_total) {
// exit immediately if index is beyond last command but we always let cmd #0 (i.e. home) be read
if (index > _cmd_total && index != 0) {
// return a command with a blank id
cmd.id = AP_MISSION_CMD_ID_NONE;
return false;
@ -723,29 +723,43 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
// check for do-jump command
if (temp_cmd.id == MAV_CMD_DO_JUMP) {
// check for invalid target
if (temp_cmd.content.jump.target >= _cmd_total) {
// To-Do: log an error?
return false;
}
// check for endless loops
if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
// we have somehow reached this jump command twice and there is no chance it will complete
// To-Do: log an error?
return false;
}else{
if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
// record this command so we can check for endless loops
jump_index = cmd_index;
}
}
// get number of times jump command has already been run
uint8_t jump_times_run = get_jump_times_run(temp_cmd);
if (jump_times_run < temp_cmd.content.jump.num_times) {
// update the record of the number of times run
if (increment_jump_num_times_if_found) {
increment_jump_times_run(temp_cmd);
}
// record this command so we can check for endless loops
if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
jump_index = cmd_index;
}
// check if jump command is 'repeat forever'
if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else{
// jump has been run specified number of times so move search to next command in mission
cmd_index++;
// get number of times jump command has already been run
int16_t jump_times_run = get_jump_times_run(temp_cmd);
if (jump_times_run < temp_cmd.content.jump.num_times) {
// update the record of the number of times run
if (increment_jump_num_times_if_found) {
increment_jump_times_run(temp_cmd);
}
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else{
// jump has been run specified number of times so move search to next command in mission
cmd_index++;
}
}
}else{
// this is a non-jump command so return it
@ -799,10 +813,10 @@ void AP_Mission::init_jump_tracking()
}
/// get_jump_times_run - returns number of times the jump command has been run
uint8_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
int16_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
{
// exit immediatley if cmd is not a do-jump command
if (cmd.id != MAV_CMD_DO_JUMP) {
// exit immediatley if cmd is not a do-jump command or target is invalid
if (cmd.id != MAV_CMD_DO_JUMP || cmd.content.jump.target >= _cmd_total) {
// To-Do: log an error?
return AP_MISSION_JUMP_TIMES_MAX;
}

8
libraries/AP_Mission/AP_Mission.h
View File

@ -32,6 +32,7 @@
#define AP_MISSION_MAX_COMMANDS ((AP_MISSION_FENCE_START_BYTE - AP_MISSION_EEPROM_START_BYTE) / AP_MISSION_EEPROM_COMMAND_SIZE) - 1 // -1 to be safe
#define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 3 // only allow up to 3 do-jump commands (due to RAM limitations on the APM2)
#define AP_MISSION_JUMP_REPEAT_FOREVER -1 // when do-jump command's repeat count is -1 this means endless repeat
#define AP_MISSION_CMD_ID_NONE 0 // mavlink cmd id of zero means invalid or missing command
#define AP_MISSION_CMD_INDEX_NONE 255 // command index of 255 means invalid or missing command
@ -49,7 +50,7 @@ public:
struct Jump_Command {
uint8_t id; // mavlink command id. To-Do: this can be removed once it is also removed from Location structure
uint8_t target; // DO_JUMP target command id
uint8_t num_times; // DO_JUMP num times to repeat
int16_t num_times; // DO_JUMP num times to repeat. -1 = repeat forever
};
union Content {
@ -224,7 +225,8 @@ private:
void init_jump_tracking();
/// get_jump_times_run - returns number of times the jump command has been run
uint8_t get_jump_times_run(const Mission_Command& cmd);
/// return is signed to be consistent with do-jump cmd's repeat count which can be -1 (to signify to repeat forever)
int16_t get_jump_times_run(const Mission_Command& cmd);
/// increment_jump_times_run - increments the recorded number of times the jump command has been run
void increment_jump_times_run(Mission_Command& cmd);
@ -248,7 +250,7 @@ private:
// jump related variables
struct jump_tracking_struct {
uint8_t index; // index of do-jump commands in mission
uint8_t num_times_run; // number of times
int16_t num_times_run; // number of times this jump command has been run
} _jump_tracking[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
};