2015-12-30 18:57:56 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2016-01-14 15:30:56 -04:00
# include "Sub.h"
2015-12-30 18:57:56 -04:00
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
2016-01-14 15:30:56 -04:00
float Sub : : get_smoothing_gain ( )
2015-12-30 18:57:56 -04:00
{
return ( 2.0f + ( float ) g . rc_feel_rp / 10.0f ) ;
}
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
2016-01-14 15:30:56 -04:00
void Sub : : get_pilot_desired_lean_angles ( float roll_in , float pitch_in , float & roll_out , float & pitch_out , float angle_max )
2015-12-30 18:57:56 -04:00
{
// sanity check angle max parameter
aparm . angle_max = constrain_int16 ( aparm . angle_max , 1000 , 8000 ) ;
// 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 ;
roll_in * = scaler ;
pitch_in * = scaler ;
// do circular limit
float total_in = pythagorous2 ( ( float ) pitch_in , ( float ) roll_in ) ;
if ( total_in > angle_max ) {
float ratio = angle_max / total_in ;
roll_in * = ratio ;
pitch_in * = ratio ;
}
// do lateral tilt to euler roll conversion
2016-02-27 15:35:34 -04:00
roll_in = ( 18000 / M_PI ) * atanf ( cosf ( pitch_in * ( M_PI / 18000 ) ) * tanf ( roll_in * ( M_PI / 18000 ) ) ) ;
2015-12-30 18:57:56 -04:00
// return
roll_out = roll_in ;
pitch_out = pitch_in ;
}
// get_pilot_desired_heading - transform pilot's yaw input into a
// desired yaw rate
// returns desired yaw rate in centi-degrees per second
2016-01-14 15:30:56 -04:00
float Sub : : get_pilot_desired_yaw_rate ( int16_t stick_angle )
2015-12-30 18:57:56 -04:00
{
// convert pilot input to the desired yaw rate
return stick_angle * g . acro_yaw_p ;
}
// check for ekf yaw reset and adjust target heading
2016-01-14 15:30:56 -04:00
void Sub : : check_ekf_yaw_reset ( )
2015-12-30 18:57:56 -04:00
{
float yaw_angle_change_rad = 0.0f ;
uint32_t new_ekfYawReset_ms = ahrs . getLastYawResetAngle ( yaw_angle_change_rad ) ;
if ( new_ekfYawReset_ms ! = ekfYawReset_ms ) {
attitude_control . shift_ef_yaw_target ( ToDeg ( yaw_angle_change_rad ) * 100.0f ) ;
ekfYawReset_ms = new_ekfYawReset_ms ;
}
}
/*************************************************************
* yaw controllers
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
2016-01-14 15:30:56 -04:00
float Sub : : get_roi_yaw ( )
2015-12-30 18:57:56 -04:00
{
static uint8_t roi_yaw_counter = 0 ; // used to reduce update rate to 100hz
roi_yaw_counter + + ;
if ( roi_yaw_counter > = 4 ) {
roi_yaw_counter = 0 ;
yaw_look_at_WP_bearing = pv_get_bearing_cd ( inertial_nav . get_position ( ) , roi_WP ) ;
}
return yaw_look_at_WP_bearing ;
}
2016-01-14 15:30:56 -04:00
float Sub : : get_look_ahead_yaw ( )
2015-12-30 18:57:56 -04:00
{
const Vector3f & vel = inertial_nav . get_velocity ( ) ;
float speed = pythagorous2 ( vel . x , vel . y ) ;
// Commanded Yaw to automatically look ahead.
if ( position_ok ( ) & & ( speed > YAW_LOOK_AHEAD_MIN_SPEED ) ) {
yaw_look_ahead_bearing = degrees ( atan2f ( vel . y , vel . x ) ) * 100.0f ;
}
return yaw_look_ahead_bearing ;
}
/*************************************************************
* throttle control
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// update_thr_average - update estimated throttle required to hover (if necessary)
// should be called at 100hz
2016-01-14 15:30:56 -04:00
void Sub : : update_thr_average ( )
2015-12-30 18:57:56 -04:00
{
// ensure throttle_average has been initialised
if ( is_zero ( throttle_average ) ) {
2016-04-05 00:17:39 -03:00
throttle_average = 0.5f ;
2015-12-30 18:57:56 -04:00
// update position controller
pos_control . set_throttle_hover ( throttle_average ) ;
}
// if not armed or landed exit
if ( ! motors . armed ( ) | | ap . land_complete ) {
return ;
}
// get throttle output
float throttle = motors . get_throttle ( ) ;
// calc average throttle if we are in a level hover
2016-04-05 00:17:39 -03:00
if ( throttle > 0.0f & & abs ( climb_rate ) < 60 & & labs ( ahrs . roll_sensor ) < 500 & & labs ( ahrs . pitch_sensor ) < 500 ) {
2015-12-30 18:57:56 -04:00
throttle_average = throttle_average * 0.99f + throttle * 0.01f ;
// update position controller
pos_control . set_throttle_hover ( throttle_average ) ;
}
}
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
2016-01-14 15:30:56 -04:00
void Sub : : set_throttle_takeoff ( )
2015-12-30 18:57:56 -04:00
{
// tell position controller to reset alt target and reset I terms
pos_control . init_takeoff ( ) ;
}
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
2016-04-05 00:17:39 -03:00
float Sub : : get_pilot_desired_throttle ( int16_t throttle_control )
2015-12-30 18:57:56 -04:00
{
2016-04-05 00:17:39 -03:00
float throttle_out ;
2015-12-30 18:57:56 -04:00
int16_t mid_stick = channel_throttle - > get_control_mid ( ) ;
// ensure reasonable throttle values
throttle_control = constrain_int16 ( throttle_control , 0 , 1000 ) ;
2016-04-05 00:17:39 -03:00
// ensure mid throttle is set within a reasonable range
2015-12-30 18:57:56 -04:00
g . throttle_mid = constrain_int16 ( g . throttle_mid , g . throttle_min + 50 , 700 ) ;
2016-04-05 00:17:39 -03:00
float thr_mid = MAX ( 0 , g . throttle_mid - g . throttle_min ) / ( float ) ( 1000 - g . throttle_min ) ;
2015-12-30 18:57:56 -04:00
// check throttle is above, below or in the deadband
if ( throttle_control < mid_stick ) {
// below the deadband
2016-04-05 00:17:39 -03:00
throttle_out = ( ( float ) throttle_control ) * thr_mid / ( float ) mid_stick ;
2015-12-30 18:57:56 -04:00
} else if ( throttle_control > mid_stick ) {
// above the deadband
2016-04-05 00:17:39 -03:00
throttle_out = ( thr_mid ) + ( ( float ) ( throttle_control - mid_stick ) ) * ( 1.0f - thr_mid ) / ( float ) ( 1000 - mid_stick ) ;
2015-12-30 18:57:56 -04:00
} else {
// must be in the deadband
2016-04-05 00:17:39 -03:00
throttle_out = thr_mid ;
2015-12-30 18:57:56 -04:00
}
return throttle_out ;
}
2016-04-05 00:17:39 -03:00
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
2015-12-30 18:57:56 -04:00
// without any deadzone at the bottom
2016-01-14 15:30:56 -04:00
float Sub : : get_pilot_desired_climb_rate ( float throttle_control )
2015-12-30 18:57:56 -04:00
{
// throttle failsafe check
if ( failsafe . radio ) {
return 0.0f ;
}
float desired_rate = 0.0f ;
float mid_stick = channel_throttle - > get_control_mid ( ) ;
float deadband_top = mid_stick + g . throttle_deadzone ;
float deadband_bottom = mid_stick - g . throttle_deadzone ;
// ensure a reasonable throttle value
2016-04-05 00:17:39 -03:00
throttle_control = constrain_float ( throttle_control , 0.0f , 1000.0f ) ;
2015-12-30 18:57:56 -04:00
// ensure a reasonable deadzone
g . throttle_deadzone = constrain_int16 ( g . throttle_deadzone , 0 , 400 ) ;
// check throttle is above, below or in the deadband
if ( throttle_control < deadband_bottom ) {
// below the deadband
2016-04-05 00:17:39 -03:00
desired_rate = g . pilot_velocity_z_max * ( throttle_control - deadband_bottom ) / deadband_bottom ;
2015-12-30 18:57:56 -04:00
} else if ( throttle_control > deadband_top ) {
// above the deadband
desired_rate = g . pilot_velocity_z_max * ( throttle_control - deadband_top ) / ( 1000.0f - deadband_top ) ;
} else {
// must be in the deadband
desired_rate = 0.0f ;
}
// desired climb rate for logging
desired_climb_rate = desired_rate ;
return desired_rate ;
}
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
2016-01-14 15:30:56 -04:00
float Sub : : get_non_takeoff_throttle ( )
2015-12-30 18:57:56 -04:00
{
2016-04-05 00:17:39 -03:00
return ( ( ( float ) g . throttle_mid / 1000.0f ) / 2.0f ) ;
2015-12-30 18:57:56 -04:00
}
2016-01-14 15:30:56 -04:00
float Sub : : get_takeoff_trigger_throttle ( )
2015-12-30 18:57:56 -04:00
{
return channel_throttle - > get_control_mid ( ) + g . takeoff_trigger_dz ;
}
2016-04-05 00:17:39 -03:00
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output (in the range 0 to 1) before take-off
2015-12-30 18:57:56 -04:00
// used only for althold, loiter, hybrid flight modes
2016-01-14 15:30:56 -04:00
float Sub : : get_throttle_pre_takeoff ( float input_thr )
2015-12-30 18:57:56 -04:00
{
// exit immediately if input_thr is zero
if ( input_thr < = 0.0f ) {
return 0.0f ;
}
2016-04-05 00:17:39 -03:00
// ensure reasonable throttle values
input_thr = constrain_float ( input_thr , 0.0f , 1000.0f ) ;
2015-12-30 18:57:56 -04:00
2016-04-05 00:17:39 -03:00
float in_min = 0.0f ;
2015-12-30 18:57:56 -04:00
float in_max = get_takeoff_trigger_throttle ( ) ;
// multicopters will output between spin-when-armed and 1/2 of mid throttle
2016-04-05 00:17:39 -03:00
float out_min = 0.0f ;
2015-12-30 18:57:56 -04:00
float out_max = get_non_takeoff_throttle ( ) ;
if ( ( g . throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK ) ! = 0 ) {
in_min = channel_throttle - > get_control_mid ( ) ;
}
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 ) {
return 0.0f ;
}
return constrain_float ( out_min + ( input_thr - in_min ) * output_range / input_range , out_min , out_max ) ;
}
// get_surface_tracking_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
2016-01-14 15:30:56 -04:00
float Sub : : get_surface_tracking_climb_rate ( int16_t target_rate , float current_alt_target , float dt )
2015-12-30 18:57:56 -04:00
{
2016-05-22 21:50:44 -03:00
# if RANGEFINDER_ENABLED == ENABLED
2015-12-30 18:57:56 -04:00
static uint32_t last_call_ms = 0 ;
float distance_error ;
float velocity_correction ;
float current_alt = inertial_nav . get_altitude ( ) ;
uint32_t now = millis ( ) ;
// reset target altitude if this controller has just been engaged
2016-05-22 21:50:44 -03:00
if ( now - last_call_ms > RANGEFINDER_TIMEOUT_MS ) {
target_rangefinder_alt = rangefinder_state . alt_cm + current_alt_target - current_alt ;
2015-12-30 18:57:56 -04:00
}
last_call_ms = now ;
2016-05-22 21:50:44 -03:00
// adjust rangefinder target alt if motors have not hit their limits
2015-12-30 18:57:56 -04:00
if ( ( target_rate < 0 & & ! motors . limit . throttle_lower ) | | ( target_rate > 0 & & ! motors . limit . throttle_upper ) ) {
2016-05-22 21:50:44 -03:00
target_rangefinder_alt + = target_rate * dt ;
2015-12-30 18:57:56 -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-22 21:50:44 -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 ( ) ) ;
2015-12-30 18:57:56 -04:00
2016-05-22 21:50:44 -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)
distance_error = ( target_rangefinder_alt - rangefinder_state . alt_cm ) - ( current_alt_target - current_alt ) ;
velocity_correction = distance_error * g . rangefinder_gain ;
2015-12-30 18:57:56 -04:00
velocity_correction = constrain_float ( velocity_correction , - THR_SURFACE_TRACKING_VELZ_MAX , THR_SURFACE_TRACKING_VELZ_MAX ) ;
2016-05-22 21:50:44 -03:00
// return combined pilot climb rate + rate to correct rangefinder alt error
2015-12-30 18:57:56 -04:00
return ( target_rate + velocity_correction ) ;
2016-05-22 21:50:44 -03:00
# else
return ( float ) target_rate ;
# endif
2015-12-30 18:57:56 -04:00
}
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
2016-04-05 02:43:42 -03:00
void Sub : : set_accel_throttle_I_from_pilot_throttle ( float pilot_throttle )
2015-12-30 18:57:56 -04:00
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
2016-04-05 00:17:39 -03:00
g . pid_accel_z . set_integrator ( ( pilot_throttle - throttle_average ) * 1000.0f ) ;
2015-12-30 18:57:56 -04:00
}
// updates position controller's maximum altitude using fence and EKF limits
2016-01-14 15:30:56 -04:00
void Sub : : update_poscon_alt_max ( )
2015-12-30 18:57:56 -04:00
{
2016-11-24 00:19:07 -04:00
// minimum altitude, ie. maximum depth
// interpreted as no limit if left as zero
float min_alt_cm = 0.0 ;
// no limit if greater than 100, a limit is necessary,
// or the vehicle will try to fly out of the water
float max_alt_cm = g . surface_depth ; // minimum depth
2015-12-30 18:57:56 -04:00
# if AC_FENCE == ENABLED
// set fence altitude limit in position controller
if ( ( fence . get_enabled_fences ( ) & AC_FENCE_TYPE_ALT_MAX ) ! = 0 ) {
2016-02-28 21:14:54 -04:00
min_alt_cm = fence . get_safe_depth ( ) * 100.0f ;
max_alt_cm = fence . get_safe_alt ( ) * 100.0f ;
2015-12-30 18:57:56 -04:00
}
# endif
// pass limit to pos controller
2016-02-28 21:14:54 -04:00
pos_control . set_alt_min ( min_alt_cm ) ;
pos_control . set_alt_max ( max_alt_cm ) ;
2015-12-30 18:57:56 -04:00
}
// rotate vector from vehicle's perspective to North-East frame
2016-01-14 15:30:56 -04:00
void Sub : : rotate_body_frame_to_NE ( float & x , float & y )
2015-12-30 18:57:56 -04:00
{
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 ;
}