From d2d166d41901f4e4aa88843da78392d075545956 Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Wed, 11 May 2011 11:55:13 +0000 Subject: [PATCH] Fix wp altitude issues git-svn-id: https://arducopter.googlecode.com/svn/trunk@2244 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 2 +- ArduCopterMega/commands.pde | 5 +++++ ArduCopterMega/commands_logic.pde | 15 ++++++++++++--- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 594e4bf816..950dacf667 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -532,7 +532,7 @@ void fast_loop() #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED // HIL for a copter needs very fast update of the servo values - gcs.send_message(MSG_RADIO_OUT); + hil.send_message(MSG_RADIO_OUT); #endif } diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index ab2eb915a5..b1be9f6df2 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -70,6 +70,11 @@ struct Location get_command_with_index(int i) temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000 } + // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative + if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ + temp.alt += home.alt; + } + if(temp.options & WP_OPTION_RELATIVE){ // If were relative, just offset from home temp.lat += home.lat; diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index fffb33df95..b191ddbe2c 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -226,7 +226,11 @@ void do_takeoff() Location temp = current_loc; // next_command.alt is a relative altitude!!! - temp.alt = next_command.alt + home.alt; + if (next_command.options & WP_OPTION_ALT_RELATIVE) { + temp.alt = next_command.alt + home.alt; + } else { + temp.alt = next_command.alt; + } takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction @@ -238,8 +242,9 @@ void do_nav_wp() { wp_control = WP_MODE; + // no longer needed as get_command_with_index takes care of this // next_command.alt is a relative altitude!!! - next_command.alt += home.alt; + // next_command.alt += home.alt; set_next_WP(&next_command); @@ -467,7 +472,11 @@ void do_change_alt() { Location temp = next_WP; condition_start = current_loc.alt; - condition_value = next_command.alt + home.alt; + if (next_command.options & WP_OPTION_ALT_RELATIVE) { + condition_value = next_command.alt + home.alt; + } else { + condition_value = next_command.alt; + } temp.alt = condition_value; set_next_WP(&temp); }