Skip to content

Commit

Permalink
Copter: move alt and wp checking to AC_WPNAV
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@rmackay9
rmackay9 committed Apr 14, 2013
1 parent 9d7d174 commit 1ee825e
Show file tree
Hide file tree
Showing 11 changed files with 199 additions and 254 deletions.
21 changes: 0 additions & 21 deletions ArduCopter/AP_State.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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){
Expand Down
48 changes: 30 additions & 18 deletions ArduCopter/ArduCopter.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)


////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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));
Expand All @@ -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;

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -1880,6 +1871,7 @@ void update_throttle_mode(void)
}
}
}
set_target_alt_for_reporting(0);
break;

case THROTTLE_MANUAL_TILT_COMPENSATED:
Expand All @@ -1904,6 +1896,7 @@ void update_throttle_mode(void)
}
}
}
set_target_alt_for_reporting(0);
break;

case THROTTLE_ACCELERATION:
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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
Expand Down
7 changes: 5 additions & 2 deletions ArduCopter/Attitude.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down Expand Up @@ -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
}
Expand Down
7 changes: 3 additions & 4 deletions ArduCopter/GCS_Mavlink.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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(
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/Log.pde
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
Loading

0 comments on commit 1ee825e

Please sign in to comment.