Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 16:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: move alt and wp checking to AC_WPNAV

Browse Source 
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
This commit is contained in:
Randy Mackay 2013-04-09 11:58:01 +09:00
parent 9d7d174995
commit 1ee825ee9a
11 changed files with 200 additions and 255 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopter/AP_State.pde
View File

@ -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){

48
ArduCopter/ArduCopter.pde
View File

@ -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

7
ArduCopter/Attitude.pde
View File

@ -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
}

7
ArduCopter/GCS_Mavlink.pde
View File

@ -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(

2
ArduCopter/Log.pde
View File

@ -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,

268
ArduCopter/commands_logic.pde
View File

@ -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()

2
ArduCopter/commands_process.pde
View File

@ -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);
}
}

17
ArduCopter/defines.h
View File

@ -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

50
ArduCopter/navigation.pde
View File

@ -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)
{

1
ArduCopter/test.pde
View File

@ -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();

32
ArduCopter/toy.pde
View File

@ -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