2011-03-19 07:20:11 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-12-03 21:54:38 -04:00
2015-05-29 23:12:49 -03:00
# include "Copter.h"
2016-06-16 23:47:59 -03:00
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw
2014-02-19 07:51:13 -04:00
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
2015-05-29 23:12:49 -03:00
float Copter : : get_smoothing_gain ( )
2014-02-12 03:28:41 -04:00
{
2014-02-19 07:51:13 -04:00
return ( 2.0f + ( float ) g . rc_feel_rp / 10.0f ) ;
2014-02-12 03:28:41 -04:00
}
2013-08-11 00:51:08 -03:00
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
2015-09-06 09:23:30 -03:00
void Copter : : get_pilot_desired_lean_angles ( float roll_in , float pitch_in , float & roll_out , float & pitch_out , float angle_max )
2013-08-11 00:51:08 -03:00
{
2015-09-06 09:23:30 -03:00
// sanity check angle max parameter
aparm . angle_max = constrain_int16 ( aparm . angle_max , 1000 , 8000 ) ;
2013-08-11 00:51:08 -03:00
2015-09-06 09:23:30 -03:00
// limit max lean angle
angle_max = constrain_float ( angle_max , 1000 , aparm . angle_max ) ;
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm . angle_max / ( float ) ROLL_PITCH_INPUT_MAX ;
2014-12-03 21:25:42 -04:00
roll_in * = scaler ;
pitch_in * = scaler ;
2014-11-10 20:06:53 -04:00
2014-12-03 21:25:42 -04:00
// do circular limit
2016-04-16 06:58:46 -03:00
float total_in = norm ( pitch_in , roll_in ) ;
2014-12-03 21:25:42 -04:00
if ( total_in > angle_max ) {
float ratio = angle_max / total_in ;
2014-11-10 20:06:53 -04:00
roll_in * = ratio ;
pitch_in * = ratio ;
}
2013-11-13 01:21:21 -04:00
2014-12-03 21:25:42 -04:00
// do lateral tilt to euler roll conversion
2016-02-25 13:13:02 -04:00
roll_in = ( 18000 / M_PI ) * atanf ( cosf ( pitch_in * ( M_PI / 18000 ) ) * tanf ( roll_in * ( M_PI / 18000 ) ) ) ;
2013-08-11 00:51:08 -03:00
2014-12-03 21:25:42 -04:00
// return
roll_out = roll_in ;
pitch_out = pitch_in ;
2013-08-11 00:51:08 -03:00
}
2015-07-26 23:03:48 -03:00
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second
2015-05-29 23:12:49 -03:00
float Copter : : get_pilot_desired_yaw_rate ( int16_t stick_angle )
2013-12-06 02:08:11 -04:00
{
// convert pilot input to the desired yaw rate
return stick_angle * g . acro_yaw_p ;
}
2015-06-23 04:49:42 -03:00
// check for ekf yaw reset and adjust target heading
void Copter : : check_ekf_yaw_reset ( )
{
float yaw_angle_change_rad = 0.0f ;
2015-09-24 03:52:19 -03:00
uint32_t new_ekfYawReset_ms = ahrs . getLastYawResetAngle ( yaw_angle_change_rad ) ;
2015-09-17 19:01:27 -03:00
if ( new_ekfYawReset_ms ! = ekfYawReset_ms ) {
2015-06-23 04:49:42 -03:00
attitude_control . shift_ef_yaw_target ( ToDeg ( yaw_angle_change_rad ) * 100.0f ) ;
2015-09-17 19:01:27 -03:00
ekfYawReset_ms = new_ekfYawReset_ms ;
2016-06-24 06:12:30 -03:00
Log_Write_Event ( DATA_EKF_YAW_RESET ) ;
2015-06-23 04:49:42 -03:00
}
}
2012-12-08 01:23:32 -04:00
/*************************************************************
* yaw controllers
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2014-02-18 08:35:29 -04:00
// get_roi_yaw - returns heading towards location held in roi_WP
2013-03-22 05:38:07 -03:00
// should be called at 100hz
2015-05-29 23:12:49 -03:00
float Copter : : get_roi_yaw ( )
2013-03-22 05:38:07 -03:00
{
2015-02-03 01:44:13 -04:00
static uint8_t roi_yaw_counter = 0 ; // used to reduce update rate to 100hz
2013-03-22 05:38:07 -03:00
2014-02-18 08:35:29 -04:00
roi_yaw_counter + + ;
2015-02-03 01:44:13 -04:00
if ( roi_yaw_counter > = 4 ) {
2014-02-18 08:35:29 -04:00
roi_yaw_counter = 0 ;
yaw_look_at_WP_bearing = pv_get_bearing_cd ( inertial_nav . get_position ( ) , roi_WP ) ;
2013-03-22 05:38:07 -03:00
}
2014-01-23 01:16:06 -04:00
return yaw_look_at_WP_bearing ;
2013-03-22 05:38:07 -03:00
}
2015-05-29 23:12:49 -03:00
float Copter : : get_look_ahead_yaw ( )
2012-12-08 01:23:32 -04:00
{
2015-04-27 13:50:14 -03:00
const Vector3f & vel = inertial_nav . get_velocity ( ) ;
2016-04-16 06:58:46 -03:00
float speed = norm ( vel . x , vel . y ) ;
2012-12-08 01:23:32 -04:00
// Commanded Yaw to automatically look ahead.
2015-04-29 00:03:34 -03:00
if ( position_ok ( ) & & ( speed > YAW_LOOK_AHEAD_MIN_SPEED ) ) {
2015-04-27 13:50:14 -03:00
yaw_look_ahead_bearing = degrees ( atan2f ( vel . y , vel . x ) ) * 100.0f ;
2012-12-08 01:23:32 -04:00
}
2014-01-23 01:16:06 -04:00
return yaw_look_ahead_bearing ;
2012-12-08 01:23:32 -04:00
}
2012-11-23 02:57:49 -04:00
/*************************************************************
* throttle control
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2016-06-09 09:42:15 -03:00
// update estimated throttle required to hover (if necessary)
// called at 100hz
void Copter : : update_throttle_hover ( )
2012-11-23 02:57:49 -04:00
{
2016-06-09 09:42:15 -03:00
# if FRAME_CONFIG != HELI_FRAME
2014-02-03 01:06:08 -04:00
// if not armed or landed exit
if ( ! motors . armed ( ) | | ap . land_complete ) {
return ;
2012-11-23 02:57:49 -04:00
}
2014-02-03 01:06:08 -04:00
2016-06-09 09:42:15 -03:00
// do not update in manual throttle modes or Drift
if ( mode_has_manual_throttle ( control_mode ) | | ( control_mode = = DRIFT ) ) {
return ;
}
// do not update while climbing or descending
if ( ! is_zero ( pos_control . get_desired_velocity ( ) . z ) ) {
return ;
}
2014-02-03 01:06:08 -04:00
// get throttle output
2015-05-23 14:51:52 -03:00
float throttle = motors . get_throttle ( ) ;
2014-02-03 01:06:08 -04:00
2012-11-24 09:50:09 -04:00
// calc average throttle if we are in a level hover
2015-12-20 07:32:52 -04:00
if ( throttle > 0.0f & & abs ( climb_rate ) < 60 & & labs ( ahrs . roll_sensor ) < 500 & & labs ( ahrs . pitch_sensor ) < 500 ) {
2016-06-09 09:42:15 -03:00
// Can we set the time constant automatically
motors . update_throttle_hover ( 0.01f ) ;
2012-11-24 00:41:17 -04:00
}
2016-06-09 09:42:15 -03:00
# endif
2012-11-23 02:57:49 -04:00
}
2013-07-26 09:43:09 -03:00
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
2015-05-29 23:12:49 -03:00
void Copter : : set_throttle_takeoff ( )
2013-07-26 09:43:09 -03:00
{
2013-12-30 09:13:27 -04:00
// tell position controller to reset alt target and reset I terms
pos_control . init_takeoff ( ) ;
2013-07-26 09:43:09 -03:00
}
2013-01-30 11:25:41 -04:00
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
2015-12-20 07:03:27 -04:00
// returns throttle output 0 to 1
float Copter : : get_pilot_desired_throttle ( int16_t throttle_control )
2013-01-30 11:25:41 -04:00
{
2015-12-20 07:03:27 -04:00
float throttle_out ;
2013-01-30 11:25:41 -04:00
2015-05-11 23:24:13 -03:00
int16_t mid_stick = channel_throttle - > get_control_mid ( ) ;
2013-01-30 11:25:41 -04:00
// ensure reasonable throttle values
2013-04-21 09:52:30 -03:00
throttle_control = constrain_int16 ( throttle_control , 0 , 1000 ) ;
2016-06-09 10:09:55 -03:00
2015-12-20 07:03:27 -04:00
// ensure mid throttle is set within a reasonable range
2016-06-09 09:42:15 -03:00
float thr_mid = constrain_float ( motors . get_throttle_hover ( ) , 0.1f , 0.9f ) ;
2013-01-30 11:25:41 -04:00
// check throttle is above, below or in the deadband
2014-11-17 18:36:13 -04:00
if ( throttle_control < mid_stick ) {
2013-01-30 11:25:41 -04:00
// below the deadband
2015-12-20 07:03:27 -04:00
throttle_out = ( ( float ) throttle_control ) * thr_mid / ( float ) mid_stick ;
2014-11-17 18:36:13 -04:00
} else if ( throttle_control > mid_stick ) {
2013-01-30 11:25:41 -04:00
// above the deadband
2015-12-20 07:03:27 -04:00
throttle_out = ( thr_mid ) + ( ( float ) ( throttle_control - mid_stick ) ) * ( 1.0f - thr_mid ) / ( float ) ( 1000 - mid_stick ) ;
2013-01-30 11:25:41 -04:00
} else {
// must be in the deadband
2015-12-20 07:03:27 -04:00
throttle_out = thr_mid ;
2013-01-30 11:25:41 -04:00
}
return throttle_out ;
}
2016-01-05 07:04:19 -04:00
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
2012-11-23 02:57:49 -04:00
// without any deadzone at the bottom
2015-05-29 23:12:49 -03:00
float Copter : : get_pilot_desired_climb_rate ( float throttle_control )
2012-11-23 02:57:49 -04:00
{
// throttle failsafe check
2013-09-26 05:54:33 -03:00
if ( failsafe . radio ) {
2015-04-30 03:10:38 -03:00
return 0.0f ;
2012-11-23 02:57:49 -04:00
}
2015-04-30 03:10:38 -03:00
float desired_rate = 0.0f ;
2015-05-19 10:38:57 -03:00
float mid_stick = channel_throttle - > get_control_mid ( ) ;
2015-04-30 03:10:38 -03:00
float deadband_top = mid_stick + g . throttle_deadzone ;
float deadband_bottom = mid_stick - g . throttle_deadzone ;
2014-11-17 18:36:13 -04:00
2012-11-23 02:57:49 -04:00
// ensure a reasonable throttle value
2016-01-05 07:04:19 -04:00
throttle_control = constrain_float ( throttle_control , 0.0f , 1000.0f ) ;
2012-11-23 02:57:49 -04:00
2014-09-02 00:38:58 -03:00
// ensure a reasonable deadzone
g . throttle_deadzone = constrain_int16 ( g . throttle_deadzone , 0 , 400 ) ;
2012-11-23 02:57:49 -04:00
// check throttle is above, below or in the deadband
2014-11-17 18:36:13 -04:00
if ( throttle_control < deadband_bottom ) {
2012-11-23 02:57:49 -04:00
// below the deadband
2016-01-05 07:04:19 -04:00
desired_rate = g . pilot_velocity_z_max * ( throttle_control - deadband_bottom ) / deadband_bottom ;
2014-11-17 18:36:13 -04:00
} else if ( throttle_control > deadband_top ) {
2012-11-23 02:57:49 -04:00
// above the deadband
2015-04-30 03:10:38 -03:00
desired_rate = g . pilot_velocity_z_max * ( throttle_control - deadband_top ) / ( 1000.0f - deadband_top ) ;
2012-11-23 02:57:49 -04:00
} else {
// must be in the deadband
2015-04-30 03:10:38 -03:00
desired_rate = 0.0f ;
2012-11-23 02:57:49 -04:00
}
2012-11-24 03:45:28 -04:00
// desired climb rate for logging
desired_climb_rate = desired_rate ;
2012-11-23 02:57:49 -04:00
return desired_rate ;
}
2014-07-17 06:15:50 -03:00
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
2015-05-29 23:12:49 -03:00
float Copter : : get_non_takeoff_throttle ( )
2014-07-17 06:15:50 -03:00
{
2016-06-09 09:42:15 -03:00
return MAX ( 0 , motors . get_throttle_hover ( ) / 2.0f ) ;
2014-07-17 06:15:50 -03:00
}
2015-05-29 23:12:49 -03:00
float Copter : : get_takeoff_trigger_throttle ( )
2015-04-30 03:04:17 -03:00
{
2015-05-19 10:38:57 -03:00
return channel_throttle - > get_control_mid ( ) + g . takeoff_trigger_dz ;
2015-04-30 03:04:17 -03:00
}
2016-01-04 07:17:27 -04:00
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off
2014-07-17 06:15:50 -03:00
// used only for althold, loiter, hybrid flight modes
2015-05-29 23:12:49 -03:00
float Copter : : get_throttle_pre_takeoff ( float input_thr )
2014-07-17 06:15:50 -03:00
{
2015-04-30 03:04:17 -03:00
// exit immediately if input_thr is zero
2015-04-30 03:25:40 -03:00
if ( input_thr < = 0.0f ) {
return 0.0f ;
2014-07-17 06:15:50 -03:00
}
2016-01-04 07:17:27 -04:00
// ensure reasonable throttle values
input_thr = constrain_float ( input_thr , 0.0f , 1000.0f ) ;
2014-11-17 18:36:13 -04:00
2016-01-04 07:17:27 -04:00
float in_min = 0.0f ;
2015-04-30 03:04:17 -03:00
float in_max = get_takeoff_trigger_throttle ( ) ;
2014-07-17 06:15:50 -03:00
2016-01-04 07:17:27 -04:00
float out_min = 0.0f ;
2015-04-30 03:04:17 -03:00
float out_max = get_non_takeoff_throttle ( ) ;
2014-07-17 06:15:50 -03:00
2015-04-30 03:04:17 -03:00
if ( ( g . throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK ) ! = 0 ) {
2015-05-19 10:38:57 -03:00
in_min = channel_throttle - > get_control_mid ( ) ;
2014-07-17 06:15:50 -03:00
}
2015-04-30 03:04:17 -03:00
float input_range = in_max - in_min ;
float output_range = out_max - out_min ;
// sanity check ranges
if ( input_range < = 0.0f | | output_range < = 0.0f ) {
2015-04-30 03:25:40 -03:00
return 0.0f ;
2014-07-17 06:15:50 -03:00
}
2015-04-30 03:04:17 -03:00
return constrain_float ( out_min + ( input_thr - in_min ) * output_range / input_range , out_min , out_max ) ;
2014-07-17 06:15:50 -03:00
}
2015-04-13 15:03:38 -03:00
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
2014-01-23 23:30:26 -04:00
// returns climb rate (in cm/s) which should be passed to the position controller
2015-05-29 23:12:49 -03:00
float Copter : : get_surface_tracking_climb_rate ( int16_t target_rate , float current_alt_target , float dt )
2012-12-29 00:51:14 -04:00
{
2016-05-12 03:44:39 -03:00
# if RANGEFINDER_ENABLED == ENABLED
2012-12-29 00:51:14 -04:00
static uint32_t last_call_ms = 0 ;
2013-01-08 03:41:07 -04:00
float distance_error ;
2013-08-18 21:52:59 -03:00
float velocity_correction ;
2015-02-10 08:28:30 -04:00
float current_alt = inertial_nav . get_altitude ( ) ;
2012-12-29 00:51:14 -04:00
uint32_t now = millis ( ) ;
// reset target altitude if this controller has just been engaged
2016-04-27 08:37:04 -03:00
if ( now - last_call_ms > RANGEFINDER_TIMEOUT_MS ) {
2016-05-07 04:55:40 -03:00
target_rangefinder_alt = rangefinder_state . alt_cm + current_alt_target - current_alt ;
2012-12-29 00:51:14 -04:00
}
2013-01-08 03:41:07 -04:00
last_call_ms = now ;
2012-12-29 00:51:14 -04:00
2016-04-27 08:37:04 -03:00
// adjust rangefinder target alt if motors have not hit their limits
2013-07-25 12:45:59 -03:00
if ( ( target_rate < 0 & & ! motors . limit . throttle_lower ) | | ( target_rate > 0 & & ! motors . limit . throttle_upper ) ) {
2016-04-27 08:37:04 -03:00
target_rangefinder_alt + = target_rate * dt ;
2013-07-25 12:45:59 -03:00
}
2012-12-29 00:51:14 -04:00
// do not let target altitude get too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
2016-05-07 04:55:40 -03:00
target_rangefinder_alt = constrain_float ( target_rangefinder_alt , rangefinder_state . alt_cm - pos_control . get_leash_down_z ( ) , rangefinder_state . alt_cm + pos_control . get_leash_up_z ( ) ) ;
2013-04-08 23:58:01 -03:00
2016-04-27 08:37:04 -03:00
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
2016-05-07 04:55:40 -03:00
distance_error = ( target_rangefinder_alt - rangefinder_state . alt_cm ) - ( current_alt_target - current_alt ) ;
2016-04-27 08:37:04 -03:00
velocity_correction = distance_error * g . rangefinder_gain ;
2013-08-18 21:52:59 -03:00
velocity_correction = constrain_float ( velocity_correction , - THR_SURFACE_TRACKING_VELZ_MAX , THR_SURFACE_TRACKING_VELZ_MAX ) ;
2012-12-29 00:51:14 -04:00
2016-04-27 08:37:04 -03:00
// return combined pilot climb rate + rate to correct rangefinder alt error
2014-01-23 23:30:26 -04:00
return ( target_rate + velocity_correction ) ;
2016-05-12 03:44:39 -03:00
# else
return ( float ) target_rate ;
# endif
2012-12-29 00:51:14 -04:00
}
2014-02-11 09:33:08 -04:00
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
2016-01-01 22:43:21 -04:00
void Copter : : set_accel_throttle_I_from_pilot_throttle ( float pilot_throttle )
2013-01-11 23:20:37 -04:00
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
2016-06-09 09:42:15 -03:00
g . pid_accel_z . set_integrator ( ( pilot_throttle - motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2013-01-11 23:20:37 -04:00
}
2015-04-23 02:57:49 -03:00
// updates position controller's maximum altitude using fence and EKF limits
2015-05-29 23:12:49 -03:00
void Copter : : update_poscon_alt_max ( )
2015-04-23 02:57:49 -03:00
{
float alt_limit_cm = 0.0f ; // interpreted as no limit if left as zero
# if AC_FENCE == ENABLED
// set fence altitude limit in position controller
if ( ( fence . get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MAX ) ! = 0 ) {
alt_limit_cm = pv_alt_above_origin ( fence . get_safe_alt ( ) * 100.0f ) ;
}
# endif
// get alt limit from EKF (limited during optical flow flight)
float ekf_limit_cm = 0.0f ;
if ( inertial_nav . get_hgt_ctrl_limit ( ekf_limit_cm ) ) {
if ( ( alt_limit_cm < = 0.0f ) | | ( ekf_limit_cm < alt_limit_cm ) ) {
alt_limit_cm = ekf_limit_cm ;
}
}
// pass limit to pos controller
pos_control . set_alt_max ( alt_limit_cm ) ;
}
2015-08-11 08:25:20 -03:00
// rotate vector from vehicle's perspective to North-East frame
void Copter : : rotate_body_frame_to_NE ( float & x , float & y )
{
float ne_x = x * ahrs . cos_yaw ( ) - y * ahrs . sin_yaw ( ) ;
float ne_y = x * ahrs . sin_yaw ( ) + y * ahrs . cos_yaw ( ) ;
x = ne_x ;
y = ne_y ;
}