diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7333038cd4..5abe716f11 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index a5f363931a..2f92d63e29 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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 ) { diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 1121aab49c..4bc6585a03 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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 // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 0b8bd14f19..4c49afedb3 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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),