From 5b93990e419f6f298bebf02155d64a4d234417e1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Mar 2013 21:59:17 +0900 Subject: [PATCH] Copter: removed next_WP Use the wp_nav.get_target_alt or controller_desired_alt in it's place --- ArduCopter/ArduCopter.pde | 38 ++++++++++++-------------------------- ArduCopter/Log.pde | 16 ++++++++-------- ArduCopter/navigation.pde | 19 ++++++++++--------- ArduCopter/test.pde | 3 +-- ArduCopter/toy.pde | 16 ++++++++-------- 5 files changed, 39 insertions(+), 53 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c4919fc1e6..518780e367 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -491,8 +491,16 @@ union float_int { //////////////////////////////////////////////////////////////////////////////// // Location & Navigation //////////////////////////////////////////////////////////////////////////////// -// This is the angle from the copter to the "next_WP" location in degrees * 100 +// This is the angle from the copter to the next waypoint in centi-degrees static int32_t wp_bearing; +// The original bearing to the next waypoint. used to check if we've passed the waypoint +static int32_t original_wp_bearing; +// The location of home in relation to the copter in centi-degrees +static int32_t home_bearing; +// distance between plane and home in cm +static int32_t home_distance; +// distance between plane and next waypoint in cm. is not static because AP_Camera uses it +uint32_t wp_distance; // navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WP static uint8_t nav_mode; // Register containing the index of the current navigation command in the mission script @@ -646,18 +654,6 @@ static int16_t angle_boost; static uint16_t land_detector; -//////////////////////////////////////////////////////////////////////////////// -// Navigation general -//////////////////////////////////////////////////////////////////////////////// -// The location of home in relation to the copter, updated every GPS read -static int32_t home_bearing; -// distance between plane and home in cm -static int32_t home_distance; -// distance between plane and next_WP in cm -// is not static because AP_Camera uses it -uint32_t wp_distance; - - //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors //////////////////////////////////////////////////////////////////////////////// @@ -666,22 +662,12 @@ uint32_t wp_distance; static struct Location home; // Current location of the copter static struct Location current_loc; -// Next WP is the desired location of the copter - the next waypoint or loiter location -static struct Location next_WP; // Holds the current loaded command from the EEPROM for navigation static struct Location command_nav_queue; // Holds the current loaded command from the EEPROM for conditional scripts static struct Location command_cond_queue; -//////////////////////////////////////////////////////////////////////////////// -// Crosstrack -//////////////////////////////////////////////////////////////////////////////// -// deg * 100, The original angle to the next_WP when the next_WP was set -// Also used to check when we pass a WP -static int32_t original_wp_bearing; - - //////////////////////////////////////////////////////////////////////////////// // Navigation Roll/Pitch functions //////////////////////////////////////////////////////////////////////////////// @@ -1806,7 +1792,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_HOLD: case THROTTLE_AUTO: controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude - set_new_altitude(current_loc.alt); // by default hold the current altitude + set_new_altitude(controller_desired_alt); // by default hold the current altitude if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I(); set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); @@ -1819,7 +1805,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) land_detector = 0; // A counter that goes up if our climb rate stalls out. controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude // Set target altitude to LAND_START_ALT if we are high, below this altitude the get_throttle_rate_stabilized will take care of setting the next_WP.alt - if (current_loc.alt >= LAND_START_ALT) { + if (controller_desired_alt >= LAND_START_ALT) { set_new_altitude(LAND_START_ALT); } throttle_initialised = true; @@ -1983,7 +1969,7 @@ void update_throttle_mode(void) case THROTTLE_AUTO: // auto pilot altitude controller with target altitude held in next_WP.alt if(motors.auto_armed() == true) { - get_throttle_althold_with_slew(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max); + get_throttle_althold_with_slew(wp_nav.get_target_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max); } break; diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 6d8d93708f..1bca767626 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -533,8 +533,8 @@ struct log_Control_Tuning { LOG_PACKET_HEADER; int16_t throttle_in; int16_t sonar_alt; - int16_t baro_alt; - int16_t next_wp_alt; + int32_t baro_alt; + float next_wp_alt; int16_t nav_throttle; int16_t angle_boost; int16_t climb_rate; @@ -549,8 +549,8 @@ static void Log_Write_Control_Tuning() LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), throttle_in : g.rc_3.control_in, sonar_alt : sonar_alt, - baro_alt : (int16_t) baro_alt, - next_wp_alt : (int16_t) next_WP.alt, + baro_alt : baro_alt, + next_wp_alt : wp_nav.get_target_alt(), nav_throttle : nav_throttle, angle_boost : angle_boost, climb_rate : climb_rate, @@ -566,12 +566,12 @@ static void Log_Read_Control_Tuning() struct log_Control_Tuning pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - // 1 2 3 4 5 6 7 8 9 - cliSerial->printf_P(PSTR("CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), + // 1 2 3 4 5 6 7 8 9 + cliSerial->printf_P(PSTR("CTUN, %d, %d, %ld, %4.0f, %d, %d, %d, %d, %d\n"), (int)pkt.throttle_in, (int)pkt.sonar_alt, - (int)pkt.baro_alt, - (int)pkt.next_wp_alt, + (long)pkt.baro_alt, + (float)pkt.next_wp_alt, (int)pkt.nav_throttle, (int)pkt.angle_boost, (int)pkt.climb_rate, diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 24d5ed6151..9304cf7df3 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -187,27 +187,28 @@ static bool check_missed_wp() return (labs(temp) > 9000); // we passed the waypoint by 90 degrees } -static void force_new_altitude(int32_t new_alt) +static void force_new_altitude(float new_alt) { - next_WP.alt = new_alt; + // update new target altitude + wp_nav.set_target_alt(new_alt); set_alt_change(REACHED_ALT); } -static void set_new_altitude(int32_t new_alt) +static void set_new_altitude(float new_alt) { // if no change exit immediately - if(new_alt == next_WP.alt) { + if(new_alt == wp_nav.get_target_alt()) { return; } // update new target altitude - next_WP.alt = new_alt; + wp_nav.set_target_alt(new_alt); - if(next_WP.alt > (current_loc.alt + 80)) { + if(new_alt > (current_loc.alt + 80)) { // we are below, going up set_alt_change(ASCENDING); - }else if(next_WP.alt < (current_loc.alt - 80)) { + }else if(new_alt < (current_loc.alt - 80)) { // we are above, going down set_alt_change(DESCENDING); @@ -222,12 +223,12 @@ static void verify_altitude() { if(alt_change_flag == ASCENDING) { // we are below, going up - if(current_loc.alt > next_WP.alt - 50) { + if(current_loc.alt > wp_nav.get_target_alt() - 50) { set_alt_change(REACHED_ALT); } }else if (alt_change_flag == DESCENDING) { // we are above, going down - if(current_loc.alt <= next_WP.alt + 50){ + if(current_loc.alt <= wp_nav.get_target_alt() + 50){ set_alt_change(REACHED_ALT); } } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 1bc0c56be5..02feaef85b 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -943,8 +943,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv) current_loc.lat = 389539260; current_loc.lng = -1199540200; - next_WP.lat = 389538528; - next_WP.lng = -1199541248; + wp_nav.set_destination(pv_latlon_to_vector(389538528,-1199541248,0)); // got 23506;, should be 22800 update_navigation(); diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index 91f2698928..3aaeb3f30a 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -52,21 +52,21 @@ void update_toy_altitude() if(false == CH7_toy_flag && input > 1666) { CH7_toy_flag = true; // go up - if(next_WP.alt >= 400) { - force_new_altitude(next_WP.alt + TOY_ALT_LARGE); + if(controller_desired_alt >= 400) { + force_new_altitude(controller_desired_alt + TOY_ALT_LARGE); }else{ - force_new_altitude(next_WP.alt + TOY_ALT_SMALL); + force_new_altitude(controller_desired_alt + TOY_ALT_SMALL); } // Trigger downward alt change }else if(false == CH7_toy_flag && input < 1333) { CH7_toy_flag = true; // go down - if(next_WP.alt >= (400 + TOY_ALT_LARGE)) { - force_new_altitude(next_WP.alt - TOY_ALT_LARGE); - }else if(next_WP.alt >= TOY_ALT_SMALL) { - force_new_altitude(next_WP.alt - TOY_ALT_SMALL); - }else if(next_WP.alt < TOY_ALT_SMALL) { + if(controller_desired_alt >= (400 + TOY_ALT_LARGE)) { + force_new_altitude(controller_desired_alt - TOY_ALT_LARGE); + }else if(controller_desired_alt >= TOY_ALT_SMALL) { + force_new_altitude(controller_desired_alt - TOY_ALT_SMALL); + }else if(controller_desired_alt < TOY_ALT_SMALL) { force_new_altitude(0); }