Mission: support do-jump num_times is -1

do-jump loops forever when num times is set to -1

libraries/AP_Mission/AP_Mission.cpp

@@ -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
+    // exit immediately if index is beyond last command but we always let cmd #0 (i.e. home) be read
-    if (index > _cmd_total) {
+    if (index > _cmd_total && index != 0) {
         // return a command with a blank id
         cmd.id = AP_MISSION_CMD_ID_NONE;
         return false;
@@ -723,19 +723,32 @@ 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
+            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{
                 // get number of times jump command has already been run
-            uint8_t jump_times_run = get_jump_times_run(temp_cmd);
+                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) {
@@ -747,6 +760,7 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
                     // 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
             cmd = temp_cmd;
@@ -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
+    // exit immediatley if cmd is not a do-jump command or target is invalid
-    if (cmd.id != MAV_CMD_DO_JUMP) {
+    if (cmd.id != MAV_CMD_DO_JUMP || cmd.content.jump.target >= _cmd_total) {
         // To-Do: log an error?
         return AP_MISSION_JUMP_TIMES_MAX;
     }

libraries/AP_Mission/AP_Mission.h

@@ -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];
 };