aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: allow accel based throttle controller to be enabled/disabled with the new THR_ACC_ENABLE parameter.

Browse Source 
When disabled the original velocity based throttle controller is used.
This commit is contained in:
rmackay9 2012-12-03 23:05:14 +09:00
parent a5bb54e36e
commit 39d524212f
4 changed files with 54 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/ArduCopter.pde
View File

@ -610,9 +610,9 @@ 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 int16_t throttle_accel_target_ef; // earth frame throttle acceleration target
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 z_accel_meas; // filtered throttle acceleration
static float throttle_avg; // g.throttle_cruise as a float
static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only
@ -1675,15 +1675,19 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
switch(new_throttle_mode) {
case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle_accel_controller_active = false; // this controller does not use accel based throttle controller
throttle_accel_deactivate(); // this controller does not use accel based throttle controller
throttle_initialised = true;
break;
case THROTTLE_ACCELERATION:
case THROTTLE_ACCELERATION: // pilot inputs the desired acceleration
if( g.throttle_accel_enabled ) { // this throttle mode requires use of the accel based throttle controller
throttle_initialised = true;
}
break;
case THROTTLE_RATE:
case THROTTLE_STABILIZED_RATE:
case THROTTLE_DIRECT_ALT:
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
@ -1693,7 +1697,6 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
}
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
@ -1701,11 +1704,11 @@ 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.
set_new_altitude(0); // Set a new target altitude
throttle_accel_controller_active = true; // this controller uses accel based throttle controller
throttle_initialised = true;
break;
default:
// To-Do: log an error message to the dataflash or tlogs instead of printing to the serial port
cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode);
break;
}
@ -1735,6 +1738,7 @@ void update_throttle_mode(void)
// do not run throttle controllers if motors disarmed
if( !motors.armed() ) {
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
return;
}
@ -1792,6 +1796,7 @@ void update_throttle_mode(void)
// pilot inputs the desired acceleration
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
}else{
int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in);
set_throttle_accel_target(desired_acceleration);
@ -1802,6 +1807,7 @@ void update_throttle_mode(void)
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate(pilot_climb_rate);
@ -1809,9 +1815,10 @@ void update_throttle_mode(void)
break;
case THROTTLE_STABILIZED_RATE:
// pilot inputs the desired climb rate. Note this is the unstabilized rate controller
// pilot inputs the desired climb rate. Note this is the stabilized rate controller
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
}else{
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
get_throttle_rate_stabilized(pilot_climb_rate);
@ -1822,6 +1829,7 @@ void update_throttle_mode(void)
// pilot inputs a desired altitude from 0 ~ 10 meters
if(g.rc_3.control_in <= 0){
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
}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);

41
ArduCopter/Attitude.pde
View File

@ -292,8 +292,8 @@ run_rate_controllers()
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 ) {
// run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
if( g.throttle_accel_enabled && throttle_accel_controller_active ) {
set_throttle_out(get_throttle_accel(throttle_accel_target_ef), true);
}
}
@ -753,7 +753,7 @@ static int16_t get_angle_boost(int16_t throttle)
}
#endif // FRAME_CONFIG == 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)
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void set_throttle_out( int16_t throttle_out, bool apply_angle_boost )
{
@ -764,16 +764,26 @@ void set_throttle_out( int16_t throttle_out, bool apply_angle_boost )
// clear angle_boost for logging purposes
angle_boost = 0;
}
// TO-DO: fix line below because this function is also called by accel based throttle controller
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;
if( g.throttle_accel_enabled ) {
throttle_accel_target_ef = desired_acceleration;
throttle_accel_controller_active = true;
}else{
// To-Do log dataflash or tlog error
cliSerial->print_P(PSTR("Err: target sent to inactive acc thr controller!\n"));
}
}
// disable_throttle_accel - disables the accel based throttle controller
// it will be re-enasbled on the next set_throttle_accel_target
// required when we wish to set motors to zero when pilot inputs zero throttle
void throttle_accel_deactivate()
{
throttle_accel_controller_active = false;
}
// get_throttle_accel - accelerometer based throttle controller
@ -928,7 +938,7 @@ 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;
int16_t target_accel;
int16_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
// calculate rate error
#if INERTIAL_NAV_Z == ENABLED
@ -949,7 +959,7 @@ get_throttle_rate(int16_t z_target_speed)
d = g.pid_throttle.get_d(z_rate_error, .02);
// consolidate target acceleration
target_accel = p+i+d;
output = p+i+d;
#if LOGGING_ENABLED == ENABLED
static uint8_t log_counter = 0; // used to slow down logging of PID values to dataflash
@ -958,13 +968,18 @@ get_throttle_rate(int16_t z_target_speed)
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, target_accel, tuning_value);
Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, output, tuning_value);
}
}
#endif
// set target for accel based throttle controller
set_throttle_accel_target(target_accel);
// send output to accelerometer based throttle controller if enabled otherwise send directly to motors
if( g.throttle_accel_enabled ) {
// set target for accel based throttle controller
set_throttle_accel_target(output);
}else{
set_throttle_out(g.throttle_cruise+output, true);
}
// update throttle cruise
if( z_target_speed == 0 ) {

2
ArduCopter/Parameters.h
View File

@ -71,6 +71,7 @@ public:
k_param_crosstrack_min_distance,
k_param_rssi_pin,
k_param_throttle_accel_enabled,
// 65: AP_Limits Library
k_param_limits = 65,
@ -269,6 +270,7 @@ public:
AP_Int8 battery_volt_pin;
AP_Int8 battery_curr_pin;
AP_Int8 rssi_pin;
AP_Int8 throttle_accel_enabled; // enable/disable accel based throttle controller
// Waypoints
//

7
ArduCopter/Parameters.pde
View File

@ -144,6 +144,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(rssi_pin, "RSSI_PIN", -1),
// @Param: THR_ACC_ENABLE
// @DisplayName: Enable Accel based throttle controller
// @Description: This allows enabling and disabling the accelerometer based throttle controller. If disabled a velocity based controller is used.
// @Values: 0:Disabled, 1:Enabled
// @User: Standard
GSCALAR(throttle_accel_enabled, "THR_ACC_ENABLE", 1),
GSCALAR(waypoint_mode, "WP_MODE", 0),
GSCALAR(command_total, "WP_TOTAL", 0),
GSCALAR(command_index, "WP_INDEX", 0),