2015-05-29 23:12:49 -03:00
# include "Copter.h"
2017-01-09 03:31:26 -04: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
2016-10-23 03:59:09 -03:00
float scaler = aparm . angle_max / ( float ) ROLL_PITCH_YAW_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
{
2016-10-23 03:55:33 -03:00
float yaw_request ;
// calculate yaw rate request
if ( g2 . acro_y_expo < = 0 ) {
yaw_request = stick_angle * g . acro_yaw_p ;
} else {
// expo variables
float y_in , y_in3 , y_out ;
// range check expo
if ( g2 . acro_y_expo > 1.0f | | g2 . acro_y_expo < 0.5f ) {
g2 . acro_y_expo = 1.0f ;
}
// yaw expo
2016-10-23 03:59:09 -03:00
y_in = float ( stick_angle ) / ROLL_PITCH_YAW_INPUT_MAX ;
2016-10-23 03:55:33 -03:00
y_in3 = y_in * y_in * y_in ;
y_out = ( g2 . acro_y_expo * y_in3 ) + ( ( 1.0f - g2 . acro_y_expo ) * y_in ) ;
2016-10-23 03:59:09 -03:00
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g . acro_yaw_p ;
2016-10-23 03:55:33 -03:00
}
2013-12-06 02:08:11 -04:00
// convert pilot input to the desired yaw rate
2016-10-23 03:55:33 -03:00
return yaw_request ;
2013-12-06 02:08:11 -04: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
2017-01-09 03:31:26 -04:00
if ( ! motors - > armed ( ) | | ap . land_complete ) {
2014-02-03 01:06:08 -04:00
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
2017-01-09 03:31:26 -04:00
if ( ! is_zero ( pos_control - > get_desired_velocity ( ) . z ) ) {
2016-06-09 09:42:15 -03:00
return ;
}
2014-02-03 01:06:08 -04:00
// get throttle output
2017-01-09 03:31:26 -04: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
2017-01-09 03:31:26 -04:00
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
2017-01-09 03:31:26 -04:00
pos_control - > init_takeoff ( ) ;
2013-07-26 09:43:09 -03:00
}
2016-06-22 01:16:28 -03:00
// transform pilot's manual throttle input to make hover throttle mid stick
2013-01-30 11:25:41 -04:00
// used only for manual throttle modes
2016-06-22 01:16:28 -03:00
// thr_mid should be in the range 0 to 1
2015-12-20 07:03:27 -04:00
// returns throttle output 0 to 1
2016-06-22 01:16:28 -03:00
float Copter : : get_pilot_desired_throttle ( int16_t throttle_control , float thr_mid )
2013-01-30 11:25:41 -04:00
{
2016-06-22 01:16:28 -03:00
if ( thr_mid < = 0.0f ) {
2017-01-09 03:31:26 -04:00
thr_mid = motors - > get_throttle_hover ( ) ;
2016-06-22 01:16:28 -03:00
}
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 ( ) ;
2016-08-08 00:14:02 -03:00
// protect against unlikely divide by zero
if ( mid_stick < = 0 ) {
mid_stick = 500 ;
}
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
2016-06-22 01:16:28 -03:00
// calculate normalised throttle input
float throttle_in ;
2014-11-17 18:36:13 -04:00
if ( throttle_control < mid_stick ) {
2013-01-30 11:25:41 -04:00
// below the deadband
2016-06-22 01:16:28 -03:00
throttle_in = ( ( float ) throttle_control ) * 0.5f / ( 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
2016-06-22 01:16:28 -03:00
throttle_in = 0.5f + ( ( float ) ( throttle_control - mid_stick ) ) * 0.5f / ( float ) ( 1000 - mid_stick ) ;
2013-01-30 11:25:41 -04:00
} else {
// must be in the deadband
2016-06-22 01:16:28 -03:00
throttle_in = 0.5f ;
2013-01-30 11:25:41 -04:00
}
2016-06-22 01:16:28 -03:00
float expo = constrain_float ( - ( thr_mid - 0.5 ) / 0.375 , - 0.5f , 1.0f ) ;
// calculate the output throttle using the given expo function
float throttle_out = throttle_in * ( 1.0f - expo ) + expo * throttle_in * throttle_in * throttle_in ;
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
{
2017-01-09 03:31:26 -04:00
return MAX ( 0 , motors - > get_throttle_hover ( ) / 2.0f ) ;
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
2017-01-09 03:31:26 -04: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
2016-11-20 02:26:51 -04:00
/*
handle rangefinder glitches . When we get a rangefinder reading
more than RANGEFINDER_GLITCH_ALT_CM different from the current
rangefinder reading then we consider it a glitch and reject
until we get RANGEFINDER_GLITCH_NUM_SAMPLES samples in a
row . When that happens we reset the target altitude to the new
reading
*/
int32_t glitch_cm = rangefinder_state . alt_cm - target_rangefinder_alt ;
if ( glitch_cm > = RANGEFINDER_GLITCH_ALT_CM ) {
rangefinder_state . glitch_count = MAX ( rangefinder_state . glitch_count + 1 , 1 ) ;
} else if ( glitch_cm < = - RANGEFINDER_GLITCH_ALT_CM ) {
rangefinder_state . glitch_count = MIN ( rangefinder_state . glitch_count - 1 , - 1 ) ;
} else {
rangefinder_state . glitch_count = 0 ;
}
if ( abs ( rangefinder_state . glitch_count ) > = RANGEFINDER_GLITCH_NUM_SAMPLES ) {
// shift to the new rangefinder reading
target_rangefinder_alt = rangefinder_state . alt_cm ;
rangefinder_state . glitch_count = 0 ;
}
if ( rangefinder_state . glitch_count ! = 0 ) {
// we are currently glitching, just use the target rate
return target_rate ;
}
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
}
2017-01-05 02:32:01 -04:00
// get target climb rate reduced to avoid obstacles and altitude fence
float Copter : : get_avoidance_adjusted_climbrate ( float target_rate )
{
# if AC_AVOID_ENABLED == ENABLED
avoid . adjust_velocity_z ( pos_control - > get_pos_z_kP ( ) , pos_control - > get_accel_z ( ) , target_rate ) ;
return target_rate ;
# else
return target_rate ;
# endif
}
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-10-14 03:34:24 -03:00
void Copter : : set_accel_throttle_I_from_pilot_throttle ( )
2013-01-11 23:20:37 -04:00
{
2016-10-14 03:34:24 -03:00
// get last throttle input sent to attitude controller
2017-01-09 03:31:26 -04:00
float pilot_throttle = constrain_float ( attitude_control - > get_throttle_in ( ) , 0.0f , 1.0f ) ;
2013-01-11 23:20:37 -04:00
// shift difference between pilot's throttle and hover throttle into accelerometer I
2017-01-09 03:31:26 -04: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
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 ;
}