From 46f292bcd860e8443b4cd6ad168a6886acd30cb2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 9 Jul 2012 23:20:43 -0700 Subject: [PATCH] Arducopter: CH7 Waypoint saving Saves the takeoff Waypoint as Home + new altitude rather than current location. --- ArduCopter/control_modes.pde | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 1022ba0eac..8b5467d04a 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -110,14 +110,16 @@ static void read_trim_switch() // increment index to WP index of 1 (home is stored at 0) CH7_wp_index = 1; + Location temp = home; // set our location ID to 16, MAV_CMD_NAV_WAYPOINT - current_loc.id = MAV_CMD_NAV_TAKEOFF; + temp.id = MAV_CMD_NAV_TAKEOFF; + temp.alt = current_loc.alt; // save command: // we use the current altitude to be the target for takeoff. // only altitude will matter to the AP mission script for takeoff. // If we are above the altitude, we will skip the command. - set_cmd_with_index(current_loc, CH7_wp_index); + set_cmd_with_index(temp, CH7_wp_index); } // increment index