2015-05-29 23:12:49 -03:00
# include "Copter.h"
2018-09-10 01:50:31 -03:00
// transform pilot's yaw input into a desired yaw rate
2015-07-26 23:03:48 -03:00
// 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-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
2019-09-07 20:33:56 -03:00
if ( flightmode - > has_manual_throttle ( ) | | ( control_mode = = Mode : : Number : : DRIFT ) ) {
2016-06-09 09:42:15 -03:00
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
2019-09-11 03:13:35 -03:00
if ( throttle > 0.0f & & fabsf ( inertial_nav . get_velocity_z ( ) ) < 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-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
2019-04-10 23:33:21 -03:00
if ( failsafe . radio ) {
2015-04-30 03:10:38 -03:00
return 0.0f ;
2012-11-23 02:57:49 -04:00
}
2018-01-18 02:49:20 -04:00
# if TOY_MODE_ENABLED == ENABLED
if ( g2 . toy_mode . enabled ( ) ) {
// allow throttle to be reduced after throttle arming and for
// slower descent close to the ground
g2 . toy_mode . throttle_adjust ( throttle_control ) ;
}
# endif
2019-04-10 23:33:21 -03:00
2015-04-30 03:10:38 -03:00
float desired_rate = 0.0f ;
2018-01-18 02:49:20 -04:00
float mid_stick = get_throttle_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
2017-11-08 09:25:53 -04:00
desired_rate = get_pilot_speed_dn ( ) * ( throttle_control - deadband_bottom ) / deadband_bottom ;
2019-04-10 23:33:21 -03:00
} else if ( throttle_control > deadband_top ) {
2012-11-23 02:57:49 -04:00
// above the deadband
2017-11-08 09:25:53 -04:00
desired_rate = g . pilot_speed_up * ( throttle_control - deadband_top ) / ( 1000.0f - deadband_top ) ;
2019-04-10 23:33:21 -03:00
} else {
2012-11-23 02:57:49 -04:00
// 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
}
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
}
2019-06-11 00:13:24 -03:00
// adjust_climb_rate - hold copter at the desired distance above the
// ground; returns climb rate (in cm/s) which should be passed to
// the position controller
2019-04-19 23:01:26 -03:00
float Copter : : SurfaceTracking : : adjust_climb_rate ( float target_rate )
2012-12-29 00:51:14 -04:00
{
2016-05-12 03:44:39 -03:00
# if RANGEFINDER_ENABLED == ENABLED
2018-06-05 07:58:21 -03:00
if ( ! copter . rangefinder_alt_ok ( ) ) {
// if rangefinder is not ok, do not use surface tracking
return target_rate ;
}
2019-06-11 00:13:24 -03:00
const float current_alt = copter . inertial_nav . get_altitude ( ) ;
const float current_alt_target = copter . pos_control - > get_alt_target ( ) ;
2013-01-08 03:41:07 -04:00
float distance_error ;
2013-08-18 21:52:59 -03:00
float velocity_correction ;
2012-12-29 00:51:14 -04:00
uint32_t now = millis ( ) ;
2019-06-11 00:13:24 -03:00
valid_for_logging = true ;
2018-06-05 05:21:09 -03:00
2012-12-29 00:51:14 -04:00
// reset target altitude if this controller has just been engaged
2019-06-11 00:13:24 -03:00
if ( now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS ) {
target_alt_cm = copter . rangefinder_state . alt_cm + current_alt_target - current_alt ;
2012-12-29 00:51:14 -04:00
}
2019-06-11 00:13:24 -03:00
last_update_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
2019-06-11 00:13:24 -03:00
if ( ( target_rate < 0 & & ! copter . motors - > limit . throttle_lower ) | | ( target_rate > 0 & & ! copter . motors - > limit . throttle_upper ) ) {
target_alt_cm + = target_rate * copter . G_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
*/
2019-06-11 00:13:24 -03:00
int32_t glitch_cm = copter . rangefinder_state . alt_cm - target_alt_cm ;
2016-11-20 02:26:51 -04:00
if ( glitch_cm > = RANGEFINDER_GLITCH_ALT_CM ) {
2019-06-11 00:13:24 -03:00
copter . rangefinder_state . glitch_count = MAX ( copter . rangefinder_state . glitch_count + 1 , 1 ) ;
2016-11-20 02:26:51 -04:00
} else if ( glitch_cm < = - RANGEFINDER_GLITCH_ALT_CM ) {
2019-06-11 00:13:24 -03:00
copter . rangefinder_state . glitch_count = MIN ( copter . rangefinder_state . glitch_count - 1 , - 1 ) ;
2016-11-20 02:26:51 -04:00
} else {
2019-06-11 00:13:24 -03:00
copter . rangefinder_state . glitch_count = 0 ;
2016-11-20 02:26:51 -04:00
}
2019-06-11 00:13:24 -03:00
if ( abs ( copter . rangefinder_state . glitch_count ) > = RANGEFINDER_GLITCH_NUM_SAMPLES ) {
2016-11-20 02:26:51 -04:00
// shift to the new rangefinder reading
2019-06-11 00:13:24 -03:00
target_alt_cm = copter . rangefinder_state . alt_cm ;
copter . rangefinder_state . glitch_count = 0 ;
2016-11-20 02:26:51 -04:00
}
2019-06-11 00:13:24 -03:00
if ( copter . rangefinder_state . glitch_count ! = 0 ) {
2016-11-20 02:26:51 -04:00
// 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)
2019-06-11 00:13:24 -03:00
distance_error = ( target_alt_cm - copter . rangefinder_state . alt_cm ) - ( current_alt_target - current_alt ) ;
velocity_correction = distance_error * copter . g . rangefinder_gain ;
2019-04-11 00:20:43 -03:00
velocity_correction = constrain_float ( velocity_correction , - SURFACE_TRACKING_VELZ_MAX , 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
2019-04-19 23:01:26 -03:00
return target_rate ;
2016-05-12 03:44:39 -03:00
# endif
2012-12-29 00:51:14 -04:00
}
2019-04-11 01:34:30 -03:00
// get surfacing tracking alt
// returns true if there is a valid target
2019-06-11 00:13:24 -03:00
bool Copter : : SurfaceTracking : : get_target_alt_cm ( float & _target_alt_cm ) const
2019-04-11 01:34:30 -03:00
{
// check target has been updated recently
2019-06-11 00:13:24 -03:00
if ( AP_HAL : : millis ( ) - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS ) {
2019-04-11 01:34:30 -03:00
return false ;
}
2019-06-11 00:13:24 -03:00
_target_alt_cm = target_alt_cm ;
2019-04-11 01:34:30 -03:00
return true ;
}
// set surface tracking target altitude
2019-06-11 00:13:24 -03:00
void Copter : : SurfaceTracking : : set_target_alt_cm ( float _target_alt_cm )
2019-04-11 01:34:30 -03:00
{
2019-06-11 00:13:24 -03:00
target_alt_cm = _target_alt_cm ;
last_update_ms = AP_HAL : : millis ( ) ;
2019-04-11 01:34:30 -03: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
2018-09-20 02:45:02 -03:00
avoid . adjust_velocity_z ( pos_control - > get_pos_z_p ( ) . kP ( ) , pos_control - > get_max_accel_z ( ) , target_rate , G_Dt ) ;
2017-01-05 02:32:01 -04:00
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-11-20 21:49:11 -04:00
pos_control - > get_accel_z_pid ( ) . 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 ;
}
2017-11-08 09:25:53 -04:00
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t Copter : : get_pilot_speed_dn ( )
{
if ( g2 . pilot_speed_dn = = 0 ) {
return abs ( g . pilot_speed_up ) ;
} else {
return abs ( g2 . pilot_speed_dn ) ;
}
}