Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: first merge of leonard's accel based altitude controller plus other changes from Randy

Browse Source 
Changes include:
New low-level get_throttle_accel function takes target acceleration and compares vs earth-frame Z accelerometer values to produce output to motors.
Higher level throttle controllers modified to call new get_throttle_accel controller
Throttle_rate_stabilized controller added which maintains a desired climb/descent rate
Throttle_land controller added - descends using normal auto throttle controller to 10m then descends at 50cm/s
Multiple throttle modes added including landing mode
Land flight mode no longer needs GPS
Throttle cruise maintenance moved to update_throttle_cruise function
This commit is contained in:
rmackay9 2012-11-23 15:57:49 +09:00
parent a83f6e54b5
commit c1ce0ae752
10 changed files with 607 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
ArduCopter/ArduCopter.pde
View File

@ -609,6 +609,15 @@ static int32_t roll_rate_target_bf = 0; // body frame roll rate target
static int32_t pitch_rate_target_bf = 0; // body frame pitch rate target
static int32_t yaw_rate_target_bf = 0; // body frame yaw rate target
////////////////////////////////////////////////////////////////////////////////
// Throttle variables
////////////////////////////////////////////////////////////////////////////////
static int16_t throttle_accel_target_ef = 0; // earth frame throttle acceleration target
static bool throttle_accel_controller_active = false; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly
static float z_accel_meas = 0; // filtered throttle acceleration
static float throttle_avg; // g.throttle_cruise as a float
////////////////////////////////////////////////////////////////////////////////
// ACRO Mode
////////////////////////////////////////////////////////////////////////////////
@ -714,8 +723,6 @@ static byte throttle_mode;
////////////////////////////////////////////////////////////////////////////////
// An additional throttle added to keep the copter at the same altitude when banking
static int16_t angle_boost;
// Push copter down for clean landing
static int16_t landing_boost;
// for controlling the landing throttle curve
//verifies landings
static int16_t ground_detector;
@ -942,6 +949,11 @@ AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENC
AP_Limit_Altitude altitude_limit(&current_loc);
#endif
////////////////////////////////////////////////////////////////////////////////
// function definitions to keep compiler from complaining about undeclared functions
////////////////////////////////////////////////////////////////////////////////
void get_throttle_althold(int32_t target_alt, int16_t max_climb_rate = ALTHOLD_MAX_CLIMB_RATE);
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
@ -1657,29 +1669,20 @@ void update_simple_mode(void)
g.rc_2.control_in = _pitch;
}
#define THROTTLE_FILTER_SIZE 2
// 50 hz update rate
// controls all throttle behavior
void update_throttle_mode(void)
{
int16_t pilot_climb_rate;
if(ap.do_flip) // this is pretty bad but needed to flip in AP modes.
return;
int16_t throttle_out;
#if AUTO_THROTTLE_HOLD != 0
static float throttle_avg = 0; // this is initialised to g.throttle_cruise later
#endif
#if FRAME_CONFIG != HELI_FRAME
// calculate angle boost
if(throttle_mode == THROTTLE_MANUAL) {
angle_boost = get_angle_boost(g.rc_3.control_in);
}else{
angle_boost = get_angle_boost(g.throttle_cruise);
// do not run throttle controllers if motors disarmed
if( !motors.armed() ) {
set_throttle_out(0);
return;
}
#endif
#if FRAME_CONFIG == HELI_FRAME
if (roll_pitch_mode == ROLL_PITCH_STABLE){
@ -1690,109 +1693,118 @@ void update_throttle_mode(void)
#endif // HELI_FRAME
switch(throttle_mode) {
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0) {
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = g.rc_3.control_in;
#else
if (control_mode == ACRO) {
g.rc_3.servo_out = g.rc_3.control_in;
}else{
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
}
#endif
// completely manual throttle
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
}else{
// send pilot's output directly to motors
set_throttle_out(g.rc_3.control_in);
#if AUTO_THROTTLE_HOLD != 0
// ensure throttle_avg has been initialised
if( throttle_avg == 0 ) {
throttle_avg = g.throttle_cruise;
}
// calc average throttle
if ((g.rc_3.control_in > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) {
throttle_avg = throttle_avg * .99 + (float)g.rc_3.control_in * .01;
g.throttle_cruise = throttle_avg;
}
#endif
// update estimate of throttle cruise
update_throttle_cruise(g.rc_3.control_in);
if (false == ap.takeoff_complete && motors.armed()) {
// check if we've taken off yet
if (!ap.takeoff_complete && motors.armed()) {
if (g.rc_3.control_in > g.throttle_cruise) {
// we must be in the air by now
set_takeoff_complete(true);
}
}
}
break;
case THROTTLE_MANUAL_TILT_COMPENSATED:
// manual throttle but with angle boost
if (g.rc_3.control_in <= 0) {
set_throttle_out(0);
}else{
// TO-DO: combine multicopter and tradheli angle boost functions
#if FRAME_CONFIG == HELI_FRAME
set_throttle_out(heli_get_angle_boost(g.rc_3.control_in));
#else
angle_boost = get_angle_boost(g.rc_3.control_in);
set_throttle_out(g.rc_3.control_in + angle_boost);
#endif
// make sure we also request 0 throttle out
// so the props stop ... properly
// ----------------------------------------
g.rc_3.servo_out = 0;
// update estimate of throttle cruise
update_throttle_cruise(g.rc_3.control_in);
if (!ap.takeoff_complete && motors.armed()) {
if (g.rc_3.control_in > g.throttle_cruise) {
// we must be in the air by now
set_takeoff_complete(true);
}
}
}
break;
case THROTTLE_ACCELERATION:
// pilot inputs the desired acceleration
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
}else{
int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in);
set_throttle_accel_target(desired_acceleration);
}
case THROTTLE_RATE:
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate(pilot_climb_rate);
}
break;
case THROTTLE_STABILIZED_RATE:
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
}
break;
case THROTTLE_DIRECT_ALT:
// pilot inputs a desired altitude from 0 ~ 10 meters
if(g.rc_3.control_in <= 0){
set_throttle_out(0);
}else{
// To-Do: this should update the global desired altitude variable next_WP.alt
int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in);
get_throttle_althold(desired_alt);
}
break;
case THROTTLE_HOLD:
// allow interactive changing of atitude
if(g.rc_3.radio_in < (g.rc_3.radio_min + 200)){
int16_t _rate = 180 - (((g.rc_3.radio_in - g.rc_3.radio_min) * 12) / 20);
reset_throttle_counter = 150;
nav_throttle = get_throttle_rate(-_rate);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
}else if(g.rc_3.radio_in > (g.rc_3.radio_max - 200)){
int16_t _rate = 300 - ((g.rc_3.radio_max - g.rc_3.radio_in) * 18) / 20;
reset_throttle_counter = 150;
nav_throttle = get_throttle_rate(_rate);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
// alt hold plus pilot input of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot override
if( pilot_climb_rate != 0 ) {
get_throttle_rate_stabilized(pilot_climb_rate);
}else{
get_throttle_rate_stabilized(0);
force_new_altitude(current_loc.alt); //TO-DO: this should be set to stabilized target
}
// allow 1 second of slow down after pilot moves throttle back into deadzone
if(reset_throttle_counter > 0) {
reset_throttle_counter--;
// if 1 second has passed set the target altitude to the current altitude
if(reset_throttle_counter == 0) {
force_new_altitude(max(current_loc.alt, 100));
}else{
nav_throttle = get_throttle_rate(0);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost;
break;
}
}
// else fall through
break;
case THROTTLE_AUTO:
// auto pilot altitude controller with target altitude held in next_WP.alt
if(motors.auto_armed() == true) {
// how far off are we
altitude_error = get_altitude_error();
//int16_t desired_climb_rate;
if(alt_change_flag == REACHED_ALT) { // we are at or above the target alt
desired_climb_rate = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error
update_throttle_cruise(g.pi_alt_hold.get_i(altitude_error, .02));
desired_climb_rate = constrain(desired_climb_rate, -250, 250);
nav_throttle = get_throttle_rate(desired_climb_rate);
}else{
desired_climb_rate = get_desired_climb_rate();
nav_throttle = get_throttle_rate(desired_climb_rate);
}
get_throttle_althold(next_WP.alt);
// TO-DO: need to somehow set nav_throttle
}
// TO-DO: what if auto_armed is not true?! throttle stuck at unknown position?
break;
// hack to remove the influence of the ground effect
if(g.sonar_enabled && current_loc.alt < 100 && landing_boost != 0) {
nav_throttle = min(nav_throttle, 0);
}
#if FRAME_CONFIG == HELI_FRAME
throttle_out = g.throttle_cruise + nav_throttle - landing_boost;
#else
throttle_out = g.throttle_cruise + nav_throttle + angle_boost - landing_boost;
#endif
g.rc_3.servo_out = throttle_out;
case THROTTLE_LAND:
// landing throttle controller
get_throttle_land();
break;
}
}

556
ArduCopter/Attitude.pde
View File

@ -291,6 +291,11 @@ run_rate_controllers()
g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf);
g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf);
#endif
// run throttle controller if an accel based target has been provided
if( throttle_accel_controller_active ) {
g.rc_3.servo_out = get_throttle_accel(throttle_accel_target_ef);
}
}
#if FRAME_CONFIG == HELI_FRAME
@ -584,11 +589,321 @@ get_rate_yaw(int32_t target_rate)
}
#endif // !HELI_FRAME
// calculate modified roll/pitch depending upon optical flow calculated position
static int32_t
get_of_roll(int32_t input_roll)
{
#ifdef OPTFLOW_ENABLED
static float tot_x_cm = 0; // total distance from target
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_roll_update) {
last_of_roll_update = optflow.last_update;
// add new distance moved
tot_x_cm += optflow.x_cm;
// only stop roll if caller isn't modifying roll
if( input_roll == 0 && current_loc.alt < 1500) {
p = g.pid_optflow_roll.get_p(-tot_x_cm);
i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0);
new_roll = p+i+d;
}else{
g.pid_optflow_roll.reset_I();
tot_x_cm = 0;
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change and maximum angle
of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_roll = constrain(of_roll, -1000, 1000);
return input_roll+of_roll;
#else
return input_roll;
#endif
}
static int32_t
get_of_pitch(int32_t input_pitch)
{
#ifdef OPTFLOW_ENABLED
static float tot_y_cm = 0; // total distance from target
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_pitch_update ) {
last_of_pitch_update = optflow.last_update;
// add new distance moved
tot_y_cm += optflow.y_cm;
// only stop roll if caller isn't modifying pitch
if( input_pitch == 0 && current_loc.alt < 1500 ) {
p = g.pid_optflow_pitch.get_p(tot_y_cm);
i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0);
new_pitch = p + i + d;
}else{
tot_y_cm = 0;
g.pid_optflow_pitch.reset_I();
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change
of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_pitch = constrain(of_pitch, -1000, 1000);
return input_pitch+of_pitch;
#else
return input_pitch;
#endif
}
/*************************************************************
* throttle control
****************************************************************/
// update_throttle_cruise - update throttle cruise if necessary
static void update_throttle_cruise(int16_t throttle)
{
// ensure throttle_avg has been initialised
if( throttle_avg == 0 ) {
throttle_avg = g.throttle_cruise;
}
// calc average throttle
if ((throttle > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) {
throttle_avg = throttle_avg * .99 + (float)throttle * .01;
g.throttle_cruise = throttle_avg;
}
}
static int16_t get_angle_boost(int16_t value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = constrain(temp, .75, 1.0);
return ((float)(value + 80) / temp) - (value + 80);
}
#if FRAME_CONFIG == HELI_FRAME
// heli_angle_boost - adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// throttle value should be 0 ~ 1000
static int16_t heli_get_angle_boost(int16_t throttle)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0);
return throttle + throttle_above_mid*angle_boost_factor;
}
#endif // HELI_FRAME
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors (instead of using accel based throttle controller)
// provide 0 to cut motors
void set_throttle_out( int16_t throttle_out )
{
g.rc_3.servo_out = throttle_out;
throttle_accel_controller_active = false;
}
// set_throttle_accel_target - to be called by upper throttle controllers to set desired vertical acceleration in earth frame
void set_throttle_accel_target( int16_t desired_acceleration )
{
throttle_accel_target_ef = desired_acceleration;
throttle_accel_controller_active = true;
}
// get_throttle_accel - accelerometer based throttle controller
// returns an actual throttle output (0 ~ 1000) to be sent to the motors
static int16_t
get_throttle_accel(int16_t z_target_accel)
{
static float accel_one_g = -980; // filtered estimate of 1G in cm/s/s
int32_t p,i,d; // used to capture pid values for logging
int16_t z_accel_error, output;
float z_accel_meas_temp;
Vector3f accel = ins.get_accel();
Matrix3f dcm_matrix = ahrs.get_dcm_matrix();
// Calculate Earth Frame Z acceleration
z_accel_meas_temp = (dcm_matrix.c.x * accel.x + dcm_matrix.c.y * accel.y + dcm_matrix.c.z * accel.z)* 100.0;
// Filter Earth Frame Z acceleration with fc = 0.01 Hz to find 1 g
accel_one_g = accel_one_g + 0.00062792 * (z_accel_meas_temp - accel_one_g);
z_accel_meas_temp = z_accel_meas_temp - accel_one_g;
// Filter Earth Frame Z acceleration with fc = 1 Hz
z_accel_meas = z_accel_meas + 0.059117 * (z_accel_meas_temp - z_accel_meas);
// calculate accel error
z_accel_error = constrain(z_target_accel + z_accel_meas, -32000, 32000);
// separately calculate p, i, d values for logging
p = g.pid_throttle_accel.get_p(z_accel_error);
// freeze I term if we've breached throttle limits
if( motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT) ) {
i = g.pid_throttle_accel.get_integrator();
}else{
i = g.pid_throttle_accel.get_i(z_accel_error, .02);
}
d = g.pid_throttle_accel.get_d(z_accel_error, .02);
//
// limit the rate
output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
//Serial.printf("ThAccel 1 z_target_accel:%4.2f z_accel_meas:%4.2f z_accel_error:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*z_accel_error, 1.0*output, 1.0*p, 1.0*i, 1.0*d);
//Serial.printf("motors.reached_limit:%4.2f reset_accel_throttle_counter:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*motors.reached_limit(0xff), 1.0*reset_accel_throttle_counter, 1.0*output, 1.0*p, 1.0*i, 1.0*d);
//Serial.printf("One G: z_target_accel:%4.2f z_accel_meas:%4.2f accel_one_g:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*accel_one_g);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
log_counter = 0;
Log_Write_PID(CH6_THROTTLE_KP, z_accel_error, p, i, d, output, tuning_value);
}
}
#endif
return output;
}
// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// without any deadzone at the bottom
#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
#define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM
#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband
#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband
static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
{
int16_t desired_rate = 0;
// throttle failsafe check
if( ap.failsafe ) {
return 0;
}
// ensure a reasonable throttle value
throttle_control = constrain(throttle_control,0,1000);
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
// below the deadband
desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
// above the deadband
desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else{
// must be in the deadband
desired_rate = 0;
}
return desired_rate;
}
// get_pilot_desired_acceleration - transform pilot's throttle input to a desired acceleration
// default upper and lower bounds are 500 cm/s/s (roughly 1/2 a G)
// returns acceleration in cm/s/s
static int16_t get_pilot_desired_acceleration(int16_t throttle_control)
{
int32_t desired_accel = 0;
// throttle failsafe check
if( ap.failsafe ) {
return 0;
}
// ensure a reasonable throttle value
throttle_control = constrain(throttle_control,0,1000);
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
// below the deadband
desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else if(throttle_control > THROTTLE_IN_DEADBAND_TOP) {
// above the deadband
desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else{
// must be in the deadband
desired_accel = 0;
}
return desired_accel;
}
// get_pilot_desired_direct_alt - transform pilot's throttle input to a desired altitude
// return altitude in cm between 0 to 10m
static int32_t get_pilot_desired_direct_alt(int16_t throttle_control)
{
int32_t desired_alt = 0;
// throttle failsafe check
if( ap.failsafe ) {
return 0;
}
// ensure a reasonable throttle value
throttle_control = constrain(throttle_control,0,1000);
desired_alt = throttle_control;
return desired_alt;
}
// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed
// sets accel based throttle controller target
static void
get_throttle_rate(int16_t z_target_speed)
{
int32_t p,i,d; // used to capture pid values for logging
int16_t z_rate_error, output = 0;
int16_t z_rate_error;
int16_t target_accel;
// calculate rate error
#if INERTIAL_NAV == ENABLED
@ -597,12 +912,6 @@ get_throttle_rate(int16_t z_target_speed)
z_rate_error = z_target_speed - climb_rate; // calc the speed error
#endif
////////////////////////////////////////////////////////////////////////////////////////
// Commenting this out because 'tmp' is not being used anymore?
// Robert Lefebvre 21/11/2012
// int16_t tmp = ((int32_t)z_target_speed * (int32_t)g.throttle_cruise) / 280;
// tmp = min(tmp, 500);
// separately calculate p, i, d values for logging
p = g.pid_throttle.get_p(z_rate_error);
@ -614,54 +923,109 @@ get_throttle_rate(int16_t z_target_speed)
}
d = g.pid_throttle.get_d(z_rate_error, .02);
//
// limit the rate
output += constrain(p+i+d, THROTTLE_RATE_CONSTRAIN_NEGATIVE, THROTTLE_RATE_CONSTRAIN_POSITIVE);
// consolidate target acceleration
target_accel = p+i+d;
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
static uint8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz
log_counter = 0;
Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, output, tuning_value);
Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, target_accel, tuning_value);
}
}
#endif
return output;
// set target for accel based throttle controller
set_throttle_accel_target(target_accel);
}
// Keeps old data out of our calculation / logs
static void reset_nav_params(void)
// get_throttle_rate_stabilized - rate controller with additional 'stabilizer'
// 'stabilizer' ensure desired rate is being met
// calls normal throttle rate controller which updates accel based throttle controller targets
static void
get_throttle_rate_stabilized(int16_t target_rate)
{
nav_throttle = 0;
static float alt_desired = 0; // The desired altitude in cm.
static float alt_rate = 0; // the desired climb rate in cm/s.
static uint32_t last_call_ms = 0;
// always start Circle mode at same angle
circle_angle = 0;
float altitude_error;
int16_t desired_rate;
int16_t alt_error_max;
uint32_t now = millis();
// We must be heading to a new WP, so XTrack must be 0
crosstrack_error = 0;
// reset target altitude if this controller has just been engaged
if( now - last_call_ms > 1000 ) {
alt_desired = current_loc.alt;
}
last_call_ms = millis();
// Will be set by new command
target_bearing = 0;
// Limit acceleration of the desired altitude to +-5 m/s^2
alt_rate += constrain(target_rate-alt_rate, -10, 10);
alt_desired += alt_rate * 0.02;
// Will be set by new command
wp_distance = 0;
alt_error_max = constrain(600-abs(target_rate),100,600);
altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max);
alt_desired = current_loc.alt + altitude_error;
// Will be set by new command, used by loiter
long_error = 0;
lat_error = 0;
nav_lon = 0;
nav_lat = 0;
nav_roll = 0;
nav_pitch = 0;
auto_roll = 0;
auto_pitch = 0;
if(altitude_error > 0){
desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error));
}else if(altitude_error < 0){
desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error)));
}else{
desired_rate = g.pi_alt_hold.get_p(0);
}
desired_rate = constrain(desired_rate, -200, 200) + target_rate;
// make sure we stick to Nav yaw on takeoff
auto_yaw = nav_yaw;
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
}
// get_throttle_althold - hold at the desired altitude in cm
// updates accel based throttle controller targets
// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller
static void
get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
{
int32_t altitude_error;
int16_t desired_rate;
altitude_error = target_alt - current_loc.alt;
if(altitude_error > 0){
desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error));
}else if(altitude_error < 0){
desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error)));
}else{
desired_rate = g.pi_alt_hold.get_p(0);
}
desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate);
// call rate based throttle controller which will update accel based throttle controller targets
get_throttle_rate(desired_rate);
// TO-DO: enabled PID logging for this controller
//Log_Write_PID(1, (int32_t)target_alt, (int32_t)current_loc.alt, (int32_t)climb_rate, (int32_t)altitude_error, (int32_t)desired_rate, (float)desired_accel);
//Log_Write_PID(1, target_alt, current_loc.alt, climb_rate, altitude_error, desired_rate, desired_accel);
}
// get_throttle_land - high level landing logic
// sends the desired acceleration in the accel based throttle controller
// called at 50hz
#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#define LAND_DESCENT_SPEED -50 // minimum descent speed in cm/s
static void
get_throttle_land()
{
// if we are above 10m perform regular alt hold descent
if (current_loc.alt >= LAND_START_ALT) {
get_throttle_althold(LAND_START_ALT, LAND_DESCENT_SPEED);
}else{
get_throttle_rate_stabilized(LAND_DESCENT_SPEED);
}
// TO-DO: add detection of whether we've landed or not and set ap.land_complete
}
/*
@ -723,125 +1087,3 @@ static void reset_stability_I(void)
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
}
/*************************************************************
* throttle control
****************************************************************/
static int16_t get_angle_boost(int16_t value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = constrain(temp, .75, 1.0);
return ((float)(value + 80) / temp) - (value + 80);
}
// calculate modified roll/pitch depending upon optical flow calculated position
static int32_t
get_of_roll(int32_t input_roll)
{
#if OPTFLOW == ENABLED
static float tot_x_cm = 0; // total distance from target
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_roll_update) {
last_of_roll_update = optflow.last_update;
// add new distance moved
tot_x_cm += optflow.x_cm;
// only stop roll if caller isn't modifying roll
if( input_roll == 0 && current_loc.alt < 1500) {
p = g.pid_optflow_roll.get_p(-tot_x_cm);
i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0);
new_roll = p+i+d;
}else{
g.pid_optflow_roll.reset_I();
tot_x_cm = 0;
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change and maximum angle
of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_roll = constrain(of_roll, -1000, 1000);
return input_roll+of_roll;
#else
return input_roll;
#endif // OPTFLOW == ENABLED
}
static int32_t
get_of_pitch(int32_t input_pitch)
{
#if OPTFLOW == ENABLED
static float tot_y_cm = 0; // total distance from target
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_pitch_update ) {
last_of_pitch_update = optflow.last_update;
// add new distance moved
tot_y_cm += optflow.y_cm;
// only stop roll if caller isn't modifying pitch
if( input_pitch == 0 && current_loc.alt < 1500 ) {
p = g.pid_optflow_pitch.get_p(tot_y_cm);
i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0);
new_pitch = p + i + d;
}else{
tot_y_cm = 0;
g.pid_optflow_pitch.reset_I();
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change
of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_pitch = constrain(of_pitch, -1000, 1000);
return input_pitch+of_pitch;
#else
return input_pitch;
#endif // OPTFLOW == ENABLED
}

3
ArduCopter/Parameters.h
View File

@ -227,6 +227,7 @@ public:
k_param_pid_optflow_pitch,
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_pid_throttle_accel, // 241
// 254,255: reserved
};
@ -368,6 +369,7 @@ public:
AC_PID pid_nav_lon;
AC_PID pid_throttle;
AC_PID pid_throttle_accel;
AC_PID pid_optflow_roll;
AC_PID pid_optflow_pitch;
@ -416,6 +418,7 @@ public:
pid_nav_lon (NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_throttle (THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_throttle_accel (THROTTLE_ACCEL_P, THROTTLE_ACCEL_I, THROTTLE_ACCEL_D, THROTTLE_ACCEL_IMAX),
pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100),

2
ArduCopter/Parameters.pde
View File

@ -371,6 +371,8 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(pid_nav_lon, "NAV_LON_", AC_PID),
GGROUP(pid_throttle, "THR_RATE_", AC_PID),
GGROUP(pid_throttle_accel,"THR_ACCEL_", AC_PID),
GGROUP(pid_optflow_roll, "OF_RLL_", AC_PID),
GGROUP(pid_optflow_pitch, "OF_PIT_", AC_PID),

76
ArduCopter/commands_logic.pde
View File

@ -148,11 +148,7 @@ static bool verify_must()
break;
case MAV_CMD_NAV_LAND:
if(g.sonar_enabled == true) {
return verify_land_sonar();
}else{
return verify_land_baro();
}
return verify_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
@ -276,14 +272,11 @@ static void do_nav_wp()
static void do_land()
{
wp_control = LOITER_MODE;
throttle_mode = THROTTLE_LAND;
// just to make sure
set_land_complete(false);
// landing boost lowers the main throttle to mimmick
// the effect of a user's hand
landing_boost = 0;
// A counter that goes up if our climb rate stalls out.
ground_detector = 0;
@ -361,74 +354,21 @@ static bool verify_takeoff()
return (current_loc.alt > next_WP.alt);
}
// called at 10hz
static bool verify_land_sonar()
// verify_land - returns true if landing has been completed
static bool verify_land()
{
// loiter above 3m
if(current_loc.alt > 300) {
wp_control = LOITER_MODE;
ground_detector = 0;
}else{
// begin to pull down on the throttle
landing_boost++;
landing_boost = min(landing_boost, 40);
}
if(current_loc.alt < 200 ) {
wp_control = NO_NAV_MODE;
}
if(current_loc.alt < 150 ) {
// if we are low or don't seem to be decending much, increment ground detector
if(current_loc.alt < 80 || abs(climb_rate) < 20) {
landing_boost++; // reduce the throttle at twice the normal rate
if(ground_detector < 30) {
ground_detector++;
}else if (ground_detector == 30) {
set_land_complete(true);
if(g.rc_3.control_in == 0) {
ground_detector++;
init_disarm_motors();
}
return true;
}
}
}
return false;
}
static bool verify_land_baro()
{
if(current_loc.alt > 300) {
wp_control = LOITER_MODE;
ground_detector = 0;
}else{
// begin to pull down on the throttle
landing_boost++;
landing_boost = min(landing_boost, 40);
}
// turn off loiter below 1m
if(current_loc.alt < 100 ) {
wp_control = NO_NAV_MODE;
}
if(current_loc.alt < 200 ) {
if(abs(climb_rate) < 40) {
landing_boost++;
if(ground_detector < 30) {
ground_detector++;
}else if (ground_detector == 30) {
set_land_complete(true);
if(g.rc_3.control_in == 0) {
ground_detector++;
init_disarm_motors();
}
return true;
}
}
}
return false;
// rely on THROTTLE_LAND mode to correctly update landing status
return ap.land_complete;
}
static bool verify_nav_wp()

32
ArduCopter/config.h
View File

@ -871,10 +871,10 @@
#endif
#ifndef ALT_HOLD_P
# define ALT_HOLD_P 0.3 // .5
# define ALT_HOLD_P 0.5
#endif
#ifndef ALT_HOLD_I
# define ALT_HOLD_I 0.04
# define ALT_HOLD_I 0.0
#endif
#ifndef ALT_HOLD_IMAX
# define ALT_HOLD_IMAX 300
@ -882,10 +882,10 @@
// RATE control
#ifndef THROTTLE_P
# define THROTTLE_P 0.3 // .25
# define THROTTLE_P 1.0
#endif
#ifndef THROTTLE_I
# define THROTTLE_I 0.03
# define THROTTLE_I 0.0
#endif
#ifndef THROTTLE_D
# define THROTTLE_D 0.0
@ -894,15 +894,27 @@
# define THROTTLE_IMAX 300
#endif
#ifndef THROTTLE_RATE_CONSTRAIN_POSITIVE
# define THROTTLE_RATE_CONSTRAIN_POSITIVE 200
// minimum and maximum climb rates while in alt hold mode
#ifndef ALTHOLD_MAX_CLIMB_RATE
# define ALTHOLD_MAX_CLIMB_RATE 500
#endif
#ifndef ALTHOLD_MIN_CLIMB_RATE
# define ALTHOLD_MIN_CLIMB_RATE -ALTHOLD_MAX_CLIMB_RATE
#endif
#ifndef THROTTLE_RATE_CONSTRAIN_NEGATIVE
# define THROTTLE_RATE_CONSTRAIN_NEGATIVE -150
// Throttle Accel control
#ifndef THROTTLE_ACCEL_P
# define THROTTLE_ACCEL_P 0.5
#endif
#ifndef THROTTLE_ACCEL_I
# define THROTTLE_ACCEL_I 1
#endif
#ifndef THROTTLE_ACCEL_D
# define THROTTLE_ACCEL_D 0.0
#endif
#ifndef THROTTLE_ACCEL_IMAX
# define THROTTLE_ACCEL_IMAX 500
#endif
//////////////////////////////////////////////////////////////////////////////

12
ArduCopter/defines.h
View File

@ -28,9 +28,15 @@
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch
// mode
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
#define THROTTLE_AUTO 2
#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors
#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation
#define THROTTLE_ACCELERATION 2 // pilot inputs the desired acceleration
#define THROTTLE_RATE 3 // pilot inputs the desired climb rate. Note: this uses the unstabilized rate controller
#define THROTTLE_STABILIZED_RATE 4 // pilot inputs the desired climb rate. Uses stabilized rate controller
#define THROTTLE_DIRECT_ALT 5 // pilot inputs a desired altitude from 0 ~ 10 meters
#define THROTTLE_HOLD 6 // alt hold plus pilot input of climb rate
#define THROTTLE_AUTO 7 // auto pilot altitude controller with target altitude held in next_WP.alt
#define THROTTLE_LAND 8 // landing throttle controller
// active altitude sensor

6
ArduCopter/flip.pde
View File

@ -39,7 +39,7 @@ void roll_flip()
if (roll < 4500) {
// Roll control
g.rc_1.servo_out = AAP_ROLL_OUT * flip_dir;
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
set_throttle_out(g.rc_3.control_in + AAP_THR_INC);
}else{
flip_state++;
}
@ -52,7 +52,7 @@ void roll_flip()
#else
g.rc_1.servo_out = get_rate_roll(40000 * flip_dir);
#endif // HELI_FRAME
g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC;
set_throttle_out(g.rc_3.control_in - AAP_THR_DEC);
}else{
flip_state++;
flip_timer = 0;
@ -63,7 +63,7 @@ void roll_flip()
if (flip_timer < 100) {
//g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
get_stabilize_roll(g.rc_1.control_in);
g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
set_throttle_out(g.rc_3.control_in + AAP_THR_INC);
flip_timer++;
}else{
Log_Write_Event(DATA_END_FLIP);

36
ArduCopter/navigation.pde
View File

@ -278,11 +278,7 @@ static void run_navigation_contollers()
break;
case LAND:
if(g.sonar_enabled)
verify_land_sonar();
else
verify_land_baro();
verify_land();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
@ -654,6 +650,36 @@ static void verify_altitude()
}
}
// Keeps old data out of our calculation / logs
static void reset_nav_params(void)
{
nav_throttle = 0;
// always start Circle mode at same angle
circle_angle = 0;
// We must be heading to a new WP, so XTrack must be 0
crosstrack_error = 0;
// Will be set by new command
target_bearing = 0;
// Will be set by new command
wp_distance = 0;
// Will be set by new command, used by loiter
long_error = 0;
lat_error = 0;
nav_lon = 0;
nav_lat = 0;
nav_roll = 0;
nav_pitch = 0;
auto_roll = 0;
auto_pitch = 0;
// make sure we stick to Nav yaw on takeoff
auto_yaw = nav_yaw;
}
static int32_t wrap_360(int32_t error)
{

50
ArduCopter/system.pde
View File

@ -390,21 +390,16 @@ static void startup_ground(void)
static void set_mode(byte mode)
{
// if we don't have GPS lock
if(false == ap.home_is_set) {
// THOR
// We don't care about Home if we don't have lock yet in Toy mode
if(mode == TOY_A || mode == TOY_M || mode == OF_LOITER) {
// nothing
}else if (mode > ALT_HOLD) {
// Switch to stabilize mode if requested mode requires a GPS lock
if(!ap.home_is_set) {
if (mode > ALT_HOLD && mode != TOY_A && mode != TOY_M && mode != OF_LOITER && mode != LAND) {
mode = STABILIZE;
}
}
// nothing but OF_LOITER for OptFlow only
if (g.optflow_enabled && g_gps->status() != GPS::GPS_OK) {
if (mode > ALT_HOLD && mode != OF_LOITER)
mode = STABILIZE;
// Switch to stabilize if OF_LOITER requested but no optical flow sensor
if (mode == OF_LOITER && !g.optflow_enabled ) {
mode = STABILIZE;
}
control_mode = mode;
@ -418,9 +413,6 @@ static void set_mode(byte mode)
// clearing value used in interactive alt hold
reset_throttle_counter = 0;
// clearing value used to force the copter down in landing mode
landing_boost = 0;
// do not auto_land if we are leaving RTL
loiter_timer = 0;
@ -458,7 +450,7 @@ static void set_mode(byte mode)
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL;
throttle_mode = THROTTLE_MANUAL_TILT_COMPENSATED;
break;
case ALT_HOLD:
@ -521,11 +513,19 @@ static void set_mode(byte mode)
break;
case LAND:
if( ap.home_is_set ) {
// switch to loiter if we have gps
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
}else{
// otherwise remain with stabilize roll and pitch
ap.manual_attitude = true;
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
}
ap.manual_throttle = false;
ap.manual_attitude = false;
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = THROTTLE_AUTO;
throttle_mode = THROTTLE_LAND;
do_land();
break;
@ -615,18 +615,6 @@ init_simple_bearing()
Log_Write_Data(DATA_INIT_SIMPLE_BEARING, initial_simple_bearing);
}
static void update_throttle_cruise(int16_t tmp)
{
if(tmp != 0) {
g.throttle_cruise += tmp;
reset_throttle_I();
}
// recalc kp
//g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0);
//cliSerial->printf("kp:%1.4f\n",kp);
}
#if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED
static boolean
check_startup_for_CLI()