From 1ee825ee9a20b14c79cc0395470dbc21b25774e4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Apr 2013 11:58:01 +0900 Subject: [PATCH] Copter: move alt and wp checking to AC_WPNAV RTL fix so that if it starts rtl-ing from above 80m it returns home while descending instead of descending at initial position. add get and set_target_alt_for_reporting --- ArduCopter/AP_State.pde | 21 --- ArduCopter/ArduCopter.pde | 48 +++--- ArduCopter/Attitude.pde | 7 +- ArduCopter/GCS_Mavlink.pde | 7 +- ArduCopter/Log.pde | 2 +- ArduCopter/commands_logic.pde | 268 +++++++++++++++++--------------- ArduCopter/commands_process.pde | 2 +- ArduCopter/defines.h | 17 +- ArduCopter/navigation.pde | 50 ------ ArduCopter/test.pde | 1 - ArduCopter/toy.pde | 32 ++-- 11 files changed, 200 insertions(+), 255 deletions(-) diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 9d8475d1d5..15071bdda9 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -119,27 +119,6 @@ void set_land_complete(bool b) // --------------------------------------------- -void set_alt_change(uint8_t flag){ - - // if no change, exit immediately - if( alt_change_flag == flag ) { - return; - } - - // update flag - alt_change_flag = flag; - - if(flag == REACHED_ALT){ - Log_Write_Event(DATA_REACHED_ALT); - - }else if(flag == ASCENDING){ - Log_Write_Event(DATA_ASCENDING); - - }else if(flag == DESCENDING){ - Log_Write_Event(DATA_DESCENDING); - } -} - void set_compass_healthy(bool b) { if(ap.compass_status != b){ diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5ac3f9429b..297dcb128b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -515,7 +515,6 @@ static uint8_t command_cond_index; // NAV_ALTITUDE - have we reached the desired altitude? // NAV_LOCATION - have we reached the desired location? // NAV_DELAY - have we waited at the waypoint the desired time? -static uint8_t wp_verify_byte; // used for tracking state of navigating waypoints static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target position static int16_t control_roll; static int16_t control_pitch; @@ -560,6 +559,7 @@ static int16_t throttle_accel_target_ef; // earth frame throttle acceleration static bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly static float throttle_avg; // g.throttle_cruise as a float static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only +static float target_alt_for_reporting; // target altitude for reporting (logs and ground station) //////////////////////////////////////////////////////////////////////////////// @@ -692,12 +692,6 @@ static int16_t nav_throttle; // 0-1000 for throttle control // This could be useful later in determining and debuging current usage and predicting battery life static uint32_t throttle_integrator; -//////////////////////////////////////////////////////////////////////////////// -// Climb rate control -//////////////////////////////////////////////////////////////////////////////// -// Time when we intiated command in millis - used for controlling decent rate -// Used to track the altitude offset for climbrate control -static int8_t alt_change_flag; //////////////////////////////////////////////////////////////////////////////// // Navigation Yaw control @@ -1789,8 +1783,8 @@ 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(controller_desired_alt); // by default hold the current altitude + controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude + wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller 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)); @@ -1802,10 +1796,6 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) set_land_complete(false); // mark landing as incomplete 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 (controller_desired_alt >= LAND_START_ALT) { - set_new_altitude(LAND_START_ALT); - } throttle_initialised = true; break; @@ -1842,6 +1832,7 @@ void update_throttle_mode(void) if( !motors.armed() ) { set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command + set_target_alt_for_reporting(0); return; } @@ -1880,6 +1871,7 @@ void update_throttle_mode(void) } } } + set_target_alt_for_reporting(0); break; case THROTTLE_MANUAL_TILT_COMPENSATED: @@ -1904,6 +1896,7 @@ void update_throttle_mode(void) } } } + set_target_alt_for_reporting(0); break; case THROTTLE_ACCELERATION: @@ -1915,6 +1908,7 @@ void update_throttle_mode(void) int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in); set_throttle_accel_target(desired_acceleration); } + set_target_alt_for_reporting(0); break; case THROTTLE_RATE: @@ -1926,6 +1920,7 @@ void update_throttle_mode(void) pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); get_throttle_rate(pilot_climb_rate); } + set_target_alt_for_reporting(0); break; case THROTTLE_STABILIZED_RATE: @@ -1934,9 +1929,10 @@ void update_throttle_mode(void) set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS + set_target_alt_for_reporting(0); }else{ pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - get_throttle_rate_stabilized(pilot_climb_rate); + get_throttle_rate_stabilized(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us } break; @@ -1946,9 +1942,11 @@ void update_throttle_mode(void) set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS + set_target_alt_for_reporting(0); }else{ int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in); get_throttle_althold_with_slew(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max); + set_target_alt_for_reporting(desired_alt); } break; @@ -1957,27 +1955,41 @@ void update_throttle_mode(void) pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) { // if sonar is ok, use surface tracking - get_throttle_surface_tracking(pilot_climb_rate); + get_throttle_surface_tracking(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us }else{ // if no sonar fall back stabilize rate controller - get_throttle_rate_stabilized(pilot_climb_rate); + get_throttle_rate_stabilized(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us } break; case THROTTLE_AUTO: - // auto pilot altitude controller with target altitude held in next_WP.alt + // auto pilot altitude controller with target altitude held in wp_nav.get_desired_alt() if(motors.auto_armed() == true) { - get_throttle_althold_with_slew(wp_nav.get_target_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max); + get_throttle_althold_with_slew(wp_nav.get_desired_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max); + set_target_alt_for_reporting(wp_nav.get_desired_alt()); // To-Do: return get_destination_alt if we are flying to a waypoint } break; case THROTTLE_LAND: // landing throttle controller get_throttle_land(); + set_target_alt_for_reporting(0); break; } } +// set_target_alt_for_reporting - set target altitude for reporting purposes (logs and gcs) +static void set_target_alt_for_reporting(float alt) +{ + target_alt_for_reporting = alt; +} + +// get_target_alt_for_reporting - returns target altitude for reporting purposes (logs and gcs) +static float get_target_alt_for_reporting() +{ + return target_alt_for_reporting; +} + static void read_AHRS(void) { // Perform IMU calculations and get attitude info diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 9fe4b3cb1f..7767d0d65f 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1157,7 +1157,8 @@ get_throttle_rate_stabilized(int16_t target_rate) // do not let target altitude get too far from current altitude controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - set_new_altitude(controller_desired_alt); + // update target altitude for reporting purposes + set_target_alt_for_reporting(controller_desired_alt); get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller } @@ -1220,7 +1221,9 @@ get_throttle_surface_tracking(int16_t target_rate) // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition target_sonar_alt = constrain(target_sonar_alt,sonar_alt-750,sonar_alt+750); controller_desired_alt = current_loc.alt+(target_sonar_alt-sonar_alt); - set_new_altitude(controller_desired_alt); + + // update target altitude for reporting purposes + set_target_alt_for_reporting(controller_desired_alt); get_throttle_althold_with_slew(controller_desired_alt, target_rate-sonar_induced_slew_rate, target_rate+sonar_induced_slew_rate); // VELZ_MAX limits how quickly we react } diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 538757c2c7..38571d0760 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1701,9 +1701,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set wp_nav's destination wp_nav.set_destination(pv_location_to_vector(tell_command)); - // set altitude target - set_new_altitude(tell_command.alt); - // verify we recevied the command mavlink_msg_mission_ack_send( chan, @@ -1718,7 +1715,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command.alt += home.alt; } - set_new_altitude(tell_command.alt); + // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode + // similar to how do_change_alt works + wp_nav.set_desired_alt(tell_command.alt); // verify we recevied the command mavlink_msg_mission_ack_send( diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index a74bd8f1f5..0765354634 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -550,7 +550,7 @@ static void Log_Write_Control_Tuning() throttle_in : g.rc_3.control_in, sonar_alt : sonar_alt, baro_alt : baro_alt, - next_wp_alt : wp_nav.get_destination_alt(), + next_wp_alt : target_alt_for_reporting, nav_throttle : nav_throttle, angle_boost : angle_boost, climb_rate : climb_rate, diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index eaf961a1fb..d58d4486e7 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -218,18 +218,10 @@ static bool verify_may() static void do_RTL(void) { // set rtl state - rtl_state = RTL_STATE_INITIAL_CLIMB; + rtl_state = RTL_STATE_START; - // set roll, pitch and yaw modes - set_roll_pitch_mode(RTL_RP); - set_yaw_mode(YAW_HOLD); // first stage of RTL is the initial climb so just hold current yaw - set_throttle_mode(RTL_THR); - - // set navigation mode - set_nav_mode(NAV_LOITER); - - // override altitude to RTL altitude - set_new_altitude(get_RTL_alt()); + // verify_RTL will do the initialisation for us + verify_RTL(); } /********************************************************************************/ @@ -248,11 +240,13 @@ static void do_takeoff() // set throttle mode to AUTO although we should already be in this mode set_throttle_mode(THROTTLE_AUTO); - // set target altitude - set_new_altitude(command_nav_queue.alt); - // set our nav mode to loiter - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_WP); + + // Set wp navigation target to safe altitude above current position + Vector3f pos = inertial_nav.get_position(); + pos.z = command_nav_queue.alt; + wp_nav.set_destination(pos); // prevent flips // To-Do: check if this is still necessary @@ -268,7 +262,6 @@ static void do_nav_wp() // set throttle mode set_throttle_mode(THROTTLE_AUTO); - set_new_altitude(command_nav_queue.alt); // set nav mode set_nav_mode(NAV_WP); @@ -280,18 +273,10 @@ static void do_nav_wp() wp_bearing = wp_nav.get_bearing_to_destination(); original_wp_bearing = wp_bearing; - // this is our bitmask to verify we have met all conditions to move on - wp_verify_byte = 0; - - // if not delay requirement, set the delay achieved bit of wp_verify_byte - if( command_nav_queue.p1 == 0 ) { - wp_verify_byte |= NAV_DELAY; - }else{ - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; - // this is the delay, stored in seconds and expanded to millis - loiter_time_max = command_nav_queue.p1; - } + // this will be used to remember the time in millis after we reach or pass the WP. + loiter_time = 0; + // this is the delay, stored in seconds and expanded to millis + loiter_time_max = command_nav_queue.p1; // reset control of yaw to default if( g.yaw_override_behaviour == YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT ) { @@ -332,21 +317,27 @@ static void do_loiter_unlimited() // set throttle mode to AUTO which, if not already active, will default to hold at our current altitude set_throttle_mode(THROTTLE_AUTO); - // set target altitude if provided - if( command_nav_queue.alt != 0 ) { - set_new_altitude(command_nav_queue.alt); + // get current position + // To-Do: change this to projection based on current location and velocity + Vector3f curr = inertial_nav.get_position(); + + // default to use position provided + Vector3f pos = pv_location_to_vector(command_nav_queue); + + // use current altitude if not provided + if( command_nav_queue.alt == 0 ) { + pos.z = curr.z; } - // if no location specified loiter at current location + // use current location if not provided if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { - set_nav_mode(NAV_LOITER); - }else{ - // location specified so fly to the target location - set_nav_mode(NAV_WP); - // Set inav navigation target - wp_nav.set_destination(pv_location_to_vector(command_nav_queue)); + pos.x = curr.x; + pos.y = curr.y; } + // start way point navigator and provide it the desired location + set_nav_mode(NAV_WP); + wp_nav.set_destination(pos); } // do_circle - initiate moving in a circle @@ -358,14 +349,14 @@ static void do_circle() // set throttle mode to AUTO which, if not already active, will default to hold at our current altitude set_throttle_mode(THROTTLE_AUTO); - // set target altitude if provided - if( command_nav_queue.alt != 0 ) { - set_new_altitude(command_nav_queue.alt); - } - // set nav mode to CIRCLE set_nav_mode(NAV_CIRCLE); + // set target altitude if provided + if( command_nav_queue.alt != 0 ) { + wp_nav.set_desired_alt(command_nav_queue.alt); + } + // override default horizontal location target if( command_nav_queue.lat != 0 || command_nav_queue.lng != 0) { circle_set_center(pv_location_to_vector(command_nav_queue), ahrs.yaw); @@ -379,7 +370,6 @@ static void do_circle() // record number of desired rotations from mission command circle_desired_rotations = command_nav_queue.p1; - } // do_loiter_time - initiate loitering at a point for a given time period @@ -391,22 +381,31 @@ static void do_loiter_time() // set throttle mode to AUTO which, if not already active, will default to hold at our current altitude set_throttle_mode(THROTTLE_AUTO); - // set target altitude if provided - if( command_nav_queue.alt != 0 ) { - set_new_altitude(command_nav_queue.alt); + + // get current position + // To-Do: change this to projection based on current location and velocity + Vector3f curr = inertial_nav.get_position(); + + // default to use position provided + Vector3f pos = pv_location_to_vector(command_nav_queue); + + // use current altitude if not provided + if( command_nav_queue.alt == 0 ) { + pos.z = curr.z; } - // if no position specificed loiter at current location + // use current location if not provided if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { - set_nav_mode(NAV_LOITER); - loiter_time = millis(); - }else{ - // location specified so fly to the target location - set_nav_mode(NAV_WP); - // Set wp navigation target - wp_nav.set_destination(pv_location_to_vector(command_nav_queue)); + pos.x = curr.x; + pos.y = curr.y; } + // start way point navigator and provide it the desired location + set_nav_mode(NAV_WP); + wp_nav.set_destination(pos); + + // setup loiter timer + loiter_time = 0; loiter_time_max = command_nav_queue.p1; // units are (seconds) } @@ -423,8 +422,8 @@ static bool verify_takeoff() // do not allow I term to build up if we have not yet taken-off return false; } - // are we above our target altitude? - return (alt_change_flag == REACHED_ALT); + // have we reached our target altitude? + return wp_nav.reached_destination(); } // verify_land - returns true if landing has been completed @@ -437,35 +436,18 @@ static bool verify_land() // verify_nav_wp - check if we have reached the next way point static bool verify_nav_wp() { - // altitude checking - if( !(wp_verify_byte & NAV_ALTITUDE) ) { - if(alt_change_flag == REACHED_ALT) { - // we have reached that altitude - wp_verify_byte |= NAV_ALTITUDE; - } + // check if we have reached the waypoint + if( !wp_nav.reached_destination() ) { + return false; } - // distance checking - if( !(wp_verify_byte & NAV_LOCATION) ) { - if(wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0) || check_missed_wp()) { - wp_verify_byte |= NAV_LOCATION; - } + // start timer if necessary + if(loiter_time == 0) { + loiter_time = millis(); } - // delay checking once we've reached the location - if( !(wp_verify_byte & NAV_DELAY) && (wp_verify_byte & NAV_LOCATION) && (wp_verify_byte & NAV_ALTITUDE) ) { - // start timer if necessary - if(loiter_time == 0) { - loiter_time = millis(); - } - - // check if timer has run out - if (((millis() - loiter_time) / 1000) >= loiter_time_max) { - wp_verify_byte |= NAV_DELAY; - } - } - - if( wp_verify_byte == (NAV_LOCATION | NAV_ALTITUDE | NAV_DELAY) ) { + // check if timer has run out + if (((millis() - loiter_time) / 1000) >= loiter_time_max) { gcs_send_text_fmt(PSTR("Reached Command #%i"),command_nav_index); copter_leds_nav_blink = 15; // Cause the CopterLEDs to blink three times to indicate waypoint reached return true; @@ -476,28 +458,24 @@ static bool verify_nav_wp() static bool verify_loiter_unlimited() { - if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { - // switch to position hold - set_nav_mode(NAV_LOITER); - } return false; } // verify_loiter_time - check if we have loitered long enough static bool verify_loiter_time() { - if(nav_mode == NAV_LOITER) { - if ((millis() - loiter_time) > loiter_time_max) { - return true; - } + // return immediately if we haven't reached our destination + if (!wp_nav.reached_destination()) { + return false; } - if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { - // reset our loiter time + + // start our loiter timer + if( loiter_time == 0 ) { loiter_time = millis(); - // switch to position hold - set_nav_mode(NAV_LOITER); } - return false; + + // check if loiter timer has run out + return ((millis() - loiter_time) > loiter_time_max); } // verify_circle - check if we have circled the point enough @@ -515,20 +493,52 @@ static bool verify_RTL() bool retval = false; switch( rtl_state ) { + case RTL_STATE_START: + // set roll, pitch and yaw modes + set_roll_pitch_mode(RTL_RP); + set_throttle_mode(RTL_THR); + // set navigation mode + set_nav_mode(NAV_WP); + + // if we are below rtl alt do initial climb + if( current_loc.alt < get_RTL_alt() ) { + // first stage of RTL is the initial climb so just hold current yaw + set_yaw_mode(YAW_HOLD); + + // get current position + // To-Do: use projection of safe stopping point based on current location and velocity + Vector3f target_pos = inertial_nav.get_position(); + target_pos.z = get_RTL_alt(); + wp_nav.set_destination(target_pos); + + // advance to next rtl state + rtl_state = RTL_STATE_INITIAL_CLIMB; + + cliSerial->printf_P(PSTR("\nRTL: initial climb to %4.2f"),target_pos.z); + }else{ + // point nose towards home + // To-Do: make this user configurable whether RTL points towards home or not + set_yaw_mode(RTL_YAW); + + // Set wp navigation target to above home + wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); + + // advance to next rtl state + rtl_state = RTL_STATE_RETURNING_HOME; + } + break; case RTL_STATE_INITIAL_CLIMB: - // rely on verify_altitude function to update alt_change_flag when we've reached the target - if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) { - // override target altitude to RTL altitude - // To-Do: figure out if we want wp_nav to be responsible for altitude - set_new_altitude(get_RTL_alt()); - - // set navigation mode + // check if we've reached the safe altitude + if (wp_nav.reached_destination()) { + // set nav mode set_nav_mode(NAV_WP); + // Set wp navigation target to above home wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); // set yaw mode + // To-Do: make this user configurable whether RTL points towards home or not set_yaw_mode(RTL_YAW); // advance to next rtl state @@ -537,16 +547,18 @@ static bool verify_RTL() break; case RTL_STATE_RETURNING_HOME: - // if we've reached home initiate loiter - if (wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0) || check_missed_wp()) { - rtl_state = RTL_STATE_LOITERING_AT_HOME; - set_nav_mode(NAV_LOITER); + // check if we've reached home + if (wp_nav.reached_destination()) { + // Note: we remain in NAV_WP nav mode which should hold us above home - // set loiter timer + // start timer rtl_loiter_start_time = millis(); // give pilot back control of yaw set_yaw_mode(YAW_HOLD); + + // advance to next rtl state + rtl_state = RTL_STATE_LOITERING_AT_HOME; } break; @@ -555,16 +567,20 @@ static bool verify_RTL() if( millis() - rtl_loiter_start_time > (uint32_t)g.rtl_loiter_time.get() ) { // initiate landing or descent if(g.rtl_alt_final == 0 || ap.failsafe_radio) { - // land + // land - this will switch us into land throttle mode and loiter nav mode and give horizontal control back to pilot do_land(); // override landing location (do_land defaults to current location) + // Note: loiter controller ignores target altitude wp_nav.set_loiter_target(Vector3f(0,0,0)); // update RTL state rtl_state = RTL_STATE_LAND; }else{ - // descend + // descend using waypoint controller if(current_loc.alt > g.rtl_alt_final) { - set_new_altitude(g.rtl_alt_final); + // set navigation mode + set_nav_mode(NAV_WP); + // Set wp navigation alt target to rtl_alt_final + wp_nav.set_destination(Vector3f(0,0,g.rtl_alt_final)); } // update RTL state rtl_state = RTL_STATE_FINAL_DESCENT; @@ -573,14 +589,9 @@ static bool verify_RTL() break; case RTL_STATE_FINAL_DESCENT: - // rely on altitude check to confirm we have reached final altitude - if(current_loc.alt <= g.rtl_alt_final || alt_change_flag == REACHED_ALT) { - // switch to regular loiter mode - set_mode(LOITER); - // set loiter target to home position (note altitude is actually ignored) - wp_nav.set_loiter_target(Vector3f(0,0,g.rtl_alt_final)); - // override altitude to RTL altitude - set_new_altitude(g.rtl_alt_final); + // check we have reached final altitude + if(current_loc.alt <= g.rtl_alt_final || wp_nav.reached_destination()) { + // indicate that we've completed RTL retval = true; } break; @@ -615,11 +626,20 @@ static void do_wait_delay() static void do_change_alt() { - // set throttle mode to AUTO - set_throttle_mode(THROTTLE_AUTO); + // adjust target appropriately for each nav mode + switch (nav_mode) { + case NAV_CIRCLE: + case NAV_LOITER: + // update loiter target altitude + wp_nav.set_desired_alt(command_cond_queue.alt); + break; - // set altitude target - set_new_altitude(command_cond_queue.alt); + case NAV_WP: + // To-Do: update waypoint nav's destination altitude + break; + } + + // To-Do: store desired altitude in a variable so that it can be verified later } static void do_within_distance() @@ -673,8 +693,8 @@ static bool verify_wait_delay() static bool verify_change_alt() { - // rely on alt change flag - return (alt_change_flag == REACHED_ALT); + // To-Do: use recorded target altitude to verify we have reached the target + return true; } static bool verify_within_distance() diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 028d096c83..51f94f7a13 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -195,7 +195,7 @@ static void exit_mission() set_mode(LAND); }else{ set_mode(LOITER); - set_new_altitude(g.rtl_alt_final); + wp_nav.set_desired_alt(g.rtl_alt_final); } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4c3bc14911..c48c094eca 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -177,12 +177,6 @@ #define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low) #define CH6_INAV_TC 32 // inertial navigation baro/accel and gps/accel time constant (1.5 = strong baro/gps correction on accel estimatehas very strong does not correct accel estimate, 7 = very weak correction) -// nav byte mask used with wp_verify_byte variable -// ----------------------------------------------- -#define NAV_LOCATION 1 -#define NAV_ALTITUDE 2 -#define NAV_DELAY 4 - // Commands - Note that APM now uses a subset of the MAVLink protocol // commands. See enum MAV_CMD in the GCS_Mavlink library @@ -219,11 +213,12 @@ #define WP_OPTION_NEXT_CMD 128 // RTL state -#define RTL_STATE_INITIAL_CLIMB 0 -#define RTL_STATE_RETURNING_HOME 1 -#define RTL_STATE_LOITERING_AT_HOME 2 -#define RTL_STATE_FINAL_DESCENT 3 -#define RTL_STATE_LAND 4 +#define RTL_STATE_START 0 +#define RTL_STATE_INITIAL_CLIMB 1 +#define RTL_STATE_RETURNING_HOME 2 +#define RTL_STATE_LOITERING_AT_HOME 3 +#define RTL_STATE_FINAL_DESCENT 4 +#define RTL_STATE_LAND 5 //repeating events #define RELAY_TOGGLE 5 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 7acb4f9131..f9a3c5fa7d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -34,9 +34,6 @@ static void run_nav_updates(void) // fetch position from inertial navigation calc_position(); - // check altitude vs target - verify_altitude(); - // calculate distance and bearing for reporting and autopilot decisions calc_distance_and_bearing(); @@ -187,53 +184,6 @@ static bool check_missed_wp() return (labs(temp) > 9000); // we passed the waypoint by 90 degrees } -static void force_new_altitude(float new_alt) -{ - // update new target altitude - wp_nav.set_destination_alt(new_alt); - set_alt_change(REACHED_ALT); -} - -static void set_new_altitude(float new_alt) -{ - // if no change exit immediately - if(new_alt == wp_nav.get_destination_alt()) { - return; - } - - // update new target altitude - wp_nav.set_destination_alt(new_alt); - - if(new_alt > (current_loc.alt + 80)) { - // we are below, going up - set_alt_change(ASCENDING); - - }else if(new_alt < (current_loc.alt - 80)) { - // we are above, going down - set_alt_change(DESCENDING); - - }else{ - // No Change - set_alt_change(REACHED_ALT); - } -} - -// verify_altitude - check if we have reached the target altitude -static void verify_altitude() -{ - if(alt_change_flag == ASCENDING) { - // we are below, going up - if(current_loc.alt > wp_nav.get_destination_alt() - 50) { - set_alt_change(REACHED_ALT); - } - }else if (alt_change_flag == DESCENDING) { - // we are above, going down - if(current_loc.alt <= wp_nav.get_destination_alt() + 50){ - set_alt_change(REACHED_ALT); - } - } -} - // Keeps old data out of our calculation / logs static void reset_nav_params(void) { diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 02feaef85b..5e92871d03 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -189,7 +189,6 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv) //static int8_t //test_toy(uint8_t argc, const Menu::arg *argv) { - set_alt_change(ASCENDING) for(altitude_error = 2000; altitude_error > -100; altitude_error--){ int16_t temp = get_desired_climb_rate(); diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index 3aaeb3f30a..8b2d2b7646 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -20,19 +20,6 @@ static const int16_t toy_lookup[] = { //called at 10hz void update_toy_throttle() { - /* - * // Disabled, now handled by TOY_A (Alt hold) and TOY_M (Manual throttle) - * if (false == CH6_toy_flag && g.rc_6.radio_in >= CH_6_PWM_TRIGGER){ - * CH6_toy_flag = true; - * throttle_mode = THROTTLE_MANUAL; - * - * }else if (CH6_toy_flag && g.rc_6.radio_in < CH_6_PWM_TRIGGER){ - * CH6_toy_flag = false; - * throttle_mode = THROTTLE_AUTO; - * set_new_altitude(current_loc.alt); - * saved_toy_throttle = g.rc_3.control_in; - * }*/ - // look for a change in throttle position to exit throttle hold if(abs(g.rc_3.control_in - saved_toy_throttle) > 40) { throttle_mode = THROTTLE_MANUAL; @@ -46,28 +33,29 @@ void update_toy_throttle() void update_toy_altitude() { int16_t input = g.rc_3.radio_in; // throttle + float current_target_alt = wp_nav.get_desired_alt(); //int16_t input = g.rc_7.radio_in; // Trigger upward alt change if(false == CH7_toy_flag && input > 1666) { CH7_toy_flag = true; // go up - if(controller_desired_alt >= 400) { - force_new_altitude(controller_desired_alt + TOY_ALT_LARGE); + if(current_target_alt >= 400) { + wp_nav.set_desired_alt(current_target_alt + TOY_ALT_LARGE); }else{ - force_new_altitude(controller_desired_alt + TOY_ALT_SMALL); + wp_nav.set_desired_alt(current_target_alt + TOY_ALT_SMALL); } // Trigger downward alt change }else if(false == CH7_toy_flag && input < 1333) { CH7_toy_flag = true; // go down - 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); + if(current_target_alt >= (400 + TOY_ALT_LARGE)) { + wp_nav.set_desired_alt(current_target_alt - TOY_ALT_LARGE); + }else if(current_target_alt >= TOY_ALT_SMALL) { + wp_nav.set_desired_alt(current_target_alt - TOY_ALT_SMALL); + }else if(current_target_alt < TOY_ALT_SMALL) { + wp_nav.set_desired_alt(0); } // clear flag