Rover: magically set home when toggling first waypoint in

Currently the first toggle will put home in rather than the first
waypoint

APMrover2/RC_Channel.cpp

@@ -78,6 +78,23 @@ void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode,
     }
 }
 
+void RC_Channel_Rover::add_waypoint_for_current_loc()
+{
+    // create new mission command
+    AP_Mission::Mission_Command cmd = {};
+
+    // set new waypoint to current location
+    cmd.content.location = rover.current_loc;
+
+    // make the new command to a waypoint
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+
+    // save command
+    if (rover.mode_auto.mission.add_cmd(cmd)) {
+        hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
+    }
+}
+
 void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
 {
     switch (ch_option) {
@@ -101,19 +118,11 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
 
             // record the waypoint if not in auto mode
             if (rover.control_mode != &rover.mode_auto) {
-                // create new mission command
-                AP_Mission::Mission_Command cmd = {};
-
-                // set new waypoint to current location
-                cmd.content.location = rover.current_loc;
-
-                // make the new command to a waypoint
-                cmd.id = MAV_CMD_NAV_WAYPOINT;
-
-                // save command
-                if (rover.mode_auto.mission.add_cmd(cmd)) {
-                    hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
+                if (rover.mode_auto.mission.num_commands() == 0) {
+                    // add a home location....
+                    add_waypoint_for_current_loc();
                 }
+                add_waypoint_for_current_loc();
             }
         }
         break;

APMrover2/RC_Channel.h

@@ -22,6 +22,7 @@ private:
     void do_aux_function_change_mode(Mode &mode,
                                      const aux_switch_pos_t ch_flag);
 
+    void add_waypoint_for_current_loc();
 };
 
 class RC_Channels_Rover : public RC_Channels