2015-05-29 23:12:49 -03:00
# include "Copter.h"
2014-04-05 11:20:39 -03:00
/*
2016-07-25 15:45:29 -03:00
* Init and run calls for PosHold flight mode
2014-07-11 02:06:53 -03:00
* PosHold tries to improve upon regular loiter by mixing the pilot input with the loiter controller
2014-04-05 11:20:39 -03:00
*/
2015-05-04 19:12:19 -03:00
# define POSHOLD_SPEED_0 10 // speed below which it is always safe to switch to loiter
2014-04-13 02:33:31 -03:00
2015-05-04 19:12:19 -03:00
// 400hz loop update rate
# define POSHOLD_BRAKE_TIME_ESTIMATE_MAX (600*4) // max number of cycles the brake will be applied before we switch to loiter
# define POSHOLD_BRAKE_TO_LOITER_TIMER (150*4) // Number of cycles to transition from brake mode to loiter mode. Must be lower than POSHOLD_LOITER_STAB_TIMER
# define POSHOLD_WIND_COMP_START_TIMER (150*4) // Number of cycles to start wind compensation update after loiter is engaged
# define POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER (50*4) // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
# define POSHOLD_SMOOTH_RATE_FACTOR 0.0125f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
# define POSHOLD_WIND_COMP_TIMER_10HZ 40 // counter value used to reduce wind compensation to 10hz
# define LOOP_RATE_FACTOR 4 // used to adapt PosHold params to loop_rate
# define TC_WIND_COMP 0.0025f // Time constant for poshold_update_wind_comp_estimate()
2014-04-13 02:33:31 -03:00
// definitions that are independent of main loop rate
2015-05-04 19:12:19 -03:00
# define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
# define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
2014-04-10 03:55:54 -03:00
2014-04-05 11:20:39 -03:00
// mission state enumeration
2014-07-11 02:06:53 -03:00
enum poshold_rp_mode {
POSHOLD_PILOT_OVERRIDE = 0 , // pilot is controlling this axis (i.e. roll or pitch)
POSHOLD_BRAKE , // this axis is braking towards zero
POSHOLD_BRAKE_READY_TO_LOITER , // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage)
POSHOLD_BRAKE_TO_LOITER , // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
POSHOLD_LOITER , // both vehicle axis are holding position
POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input)
2014-04-05 11:20:39 -03:00
} ;
static struct {
2014-07-11 02:06:53 -03:00
poshold_rp_mode roll_mode : 3 ; // roll mode: pilot override, brake or loiter
poshold_rp_mode pitch_mode : 3 ; // pitch mode: pilot override, brake or loiter
2014-04-11 05:16:40 -03:00
uint8_t braking_time_updated_roll : 1 ; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1 ; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
// pilot input related variables
2014-12-03 21:25:42 -04:00
float pilot_roll ; // pilot requested roll angle (filtered to slow returns to zero)
float pilot_pitch ; // pilot requested roll angle (filtered to slow returns to zero)
2014-04-11 05:16:40 -03:00
// braking related variables
float brake_gain ; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate)
int16_t brake_roll ; // target roll angle during braking periods
int16_t brake_pitch ; // target pitch angle during braking periods
int16_t brake_timeout_roll ; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t brake_timeout_pitch ; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t brake_angle_max_roll ; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
int16_t brake_angle_max_pitch ; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
2014-07-11 02:06:53 -03:00
int16_t brake_to_loiter_timer ; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER
2014-04-11 05:16:40 -03:00
// loiter related variables
2014-07-11 02:06:53 -03:00
int16_t controller_to_pilot_timer_roll ; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
2015-01-06 01:44:52 -04:00
int16_t controller_to_pilot_timer_pitch ; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
2014-04-23 00:39:46 -03:00
int16_t controller_final_roll ; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
int16_t controller_final_pitch ; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
2014-04-11 05:16:40 -03:00
// wind compensation related variables
Vector2f wind_comp_ef ; // wind compensation in earth frame, filtered lean angles from position controller
int16_t wind_comp_roll ; // roll angle to compensate for wind
int16_t wind_comp_pitch ; // pitch angle to compensate for wind
2015-01-06 01:44:52 -04:00
uint16_t wind_comp_start_timer ; // counter to delay start of wind compensation for a short time after loiter is engaged
int8_t wind_comp_timer ; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
2014-04-11 05:16:40 -03:00
// final output
int16_t roll ; // final roll angle sent to attitude controller
int16_t pitch ; // final pitch angle sent to attitude controller
2014-07-11 02:06:53 -03:00
} poshold ;
2014-04-04 11:14:07 -03:00
2014-07-11 02:06:53 -03:00
// poshold_init - initialise PosHold controller
2017-12-10 23:51:13 -04:00
bool Copter : : ModePosHold : : init ( bool ignore_checks )
2014-04-04 11:14:07 -03:00
{
2014-07-11 02:06:53 -03:00
// fail to initialise PosHold mode if no GPS lock
2018-02-07 22:21:09 -04:00
if ( ! copter . position_ok ( ) & & ! ignore_checks ) {
2014-04-10 03:55:54 -03:00
return false ;
}
Copter: Hybrid fixes to wind_comp, brake pitch timer, thr peaks
There was an error in the velocity axis used to update
brake_timeout_pitch (vel_right instead of vel_fw)
The wind_comp was not enough filtered for the Pixhawk (400Hz), so I
added a specific time constant (TC_WIND_COMP) to have the expected
filter with 400Hz controllers.
About throttle peaks, after some tests and from logs, they happen when
hybrid switches to loiter.
There is always a difference between Alt and DesiredAlt (DAlt), but,
when loiter engages, it initializes DAlt = Alt and the copter tries
immediatelly to reach that new setpoint. So the solution would be to
init_loiter_target() just as it was in pre-onion code : only x/y and not
z. and to be able to pass parameters like that
wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Well, from this new code structure, it seems not possible with current
functions so I've used set_loiter_target that init position passed as
parameter and velocity to 0 (as expected).
BTW, I think there was something wrong with set_loiter_target function,
the "Vector3f& position" parameter was not used at all...
I moved the reset flag from init_loiter_target to set_loiter_target.
2014-04-23 01:40:03 -03:00
2015-11-18 09:07:42 -04:00
// initialize vertical speeds and acceleration
2017-11-08 09:25:53 -04:00
pos_control - > set_speed_z ( - get_pilot_speed_dn ( ) , g . pilot_speed_up ) ;
2017-01-09 03:31:26 -04:00
pos_control - > set_accel_z ( g . pilot_accel_z ) ;
2014-04-04 11:14:07 -03:00
2015-10-27 10:06:50 -03:00
// initialise position and desired velocity
2017-01-09 03:31:26 -04:00
if ( ! pos_control - > is_active_z ( ) ) {
pos_control - > set_alt_target_to_current_alt ( ) ;
pos_control - > set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
2016-10-14 09:28:32 -03:00
}
2014-04-30 04:29:32 -03:00
2014-04-11 05:16:40 -03:00
// initialise lean angles to current attitude
2014-07-11 02:06:53 -03:00
poshold . pilot_roll = 0 ;
poshold . pilot_pitch = 0 ;
2014-04-11 05:16:40 -03:00
2014-04-10 03:55:54 -03:00
// compute brake_gain
2014-07-11 02:06:53 -03:00
poshold . brake_gain = ( 15.0f * ( float ) g . poshold_brake_rate + 95.0f ) / 100.0f ;
2014-04-05 11:20:39 -03:00
2014-04-10 03:55:54 -03:00
if ( ap . land_complete ) {
2014-04-11 05:16:40 -03:00
// if landed begin in loiter mode
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_LOITER ;
poshold . pitch_mode = POSHOLD_LOITER ;
Copter: Hybrid fixes to wind_comp, brake pitch timer, thr peaks
There was an error in the velocity axis used to update
brake_timeout_pitch (vel_right instead of vel_fw)
The wind_comp was not enough filtered for the Pixhawk (400Hz), so I
added a specific time constant (TC_WIND_COMP) to have the expected
filter with 400Hz controllers.
About throttle peaks, after some tests and from logs, they happen when
hybrid switches to loiter.
There is always a difference between Alt and DesiredAlt (DAlt), but,
when loiter engages, it initializes DAlt = Alt and the copter tries
immediatelly to reach that new setpoint. So the solution would be to
init_loiter_target() just as it was in pre-onion code : only x/y and not
z. and to be able to pass parameters like that
wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Well, from this new code structure, it seems not possible with current
functions so I've used set_loiter_target that init position passed as
parameter and velocity to 0 (as expected).
BTW, I think there was something wrong with set_loiter_target function,
the "Vector3f& position" parameter was not used at all...
I moved the reset flag from init_loiter_target to set_loiter_target.
2014-04-23 01:40:03 -03:00
// set target to current position
2014-07-11 02:06:53 -03:00
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
2018-03-27 23:13:37 -03:00
loiter_nav - > init_target ( ) ;
2014-04-04 11:14:07 -03:00
} else {
2014-04-11 05:16:40 -03:00
// if not landed start in pilot override to avoid hard twitch
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_PILOT_OVERRIDE ;
poshold . pitch_mode = POSHOLD_PILOT_OVERRIDE ;
2014-04-04 11:14:07 -03:00
}
2014-04-10 03:55:54 -03:00
2014-07-11 02:06:53 -03:00
// initialise wind_comp each time PosHold is switched on
poshold . wind_comp_ef . zero ( ) ;
poshold . wind_comp_roll = 0 ;
poshold . wind_comp_pitch = 0 ;
poshold . wind_comp_timer = 0 ;
2014-04-10 03:55:54 -03:00
return true ;
2014-04-04 11:14:07 -03:00
}
2014-07-11 02:06:53 -03:00
// poshold_run - runs the PosHold controller
2014-04-04 11:14:07 -03:00
// should be called at 100hz or more
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : run ( )
2014-04-04 11:14:07 -03:00
{
2014-12-03 21:25:42 -04:00
float target_roll , target_pitch ; // pilot's roll and pitch angle inputs
2014-04-10 03:55:54 -03:00
float target_yaw_rate = 0 ; // pilot desired yaw rate in centi-degrees/sec
2015-04-30 04:40:38 -03:00
float target_climb_rate = 0 ; // pilot desired climb rate in centimeters/sec
float takeoff_climb_rate = 0.0f ; // takeoff induced climb rate
2014-04-11 05:16:40 -03:00
float brake_to_loiter_mix ; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
2014-04-23 00:39:46 -03:00
float controller_to_pilot_roll_mix ; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix ; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
2014-04-10 03:55:54 -03:00
float vel_fw , vel_right ; // vehicle's current velocity in body-frame forward and right directions
2014-04-05 11:20:39 -03:00
const Vector3f & vel = inertial_nav . get_velocity ( ) ;
2014-04-04 11:14:07 -03:00
2015-11-18 09:07:42 -04:00
// initialize vertical speeds and acceleration
2017-11-08 09:25:53 -04:00
pos_control - > set_speed_z ( - get_pilot_speed_dn ( ) , g . pilot_speed_up ) ;
2017-01-09 03:31:26 -04:00
pos_control - > set_accel_z ( g . pilot_accel_z ) ;
2015-10-19 21:05:10 -03:00
2015-04-17 14:49:08 -03:00
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
2017-01-09 03:31:26 -04:00
if ( ! motors - > armed ( ) | | ! ap . auto_armed | | ! motors - > get_interlock ( ) ) {
2018-03-27 23:13:37 -03:00
loiter_nav - > init_target ( ) ;
2017-12-28 22:37:22 -04:00
zero_throttle_and_relax_ac ( ) ;
2017-01-09 03:31:26 -04:00
pos_control - > relax_alt_hold_controllers ( 0.0f ) ;
2014-04-04 11:14:07 -03:00
return ;
}
// process pilot inputs
2018-02-07 22:21:09 -04:00
if ( ! copter . failsafe . radio ) {
2014-04-04 11:14:07 -03:00
// apply SIMPLE mode transform to pilot inputs
update_simple_mode ( ) ;
// get pilot's desired yaw rate
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_yaw_rate = get_pilot_desired_yaw_rate ( channel_yaw - > get_control_in ( ) ) ;
2014-04-04 11:14:07 -03:00
// get pilot desired climb rate (for alt-hold mode and take-off)
ArduCopter: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
target_climb_rate = get_pilot_desired_climb_rate ( channel_throttle - > get_control_in ( ) ) ;
2017-11-08 09:25:53 -04:00
target_climb_rate = constrain_float ( target_climb_rate , - get_pilot_speed_dn ( ) , g . pilot_speed_up ) ;
2015-04-30 04:40:38 -03:00
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates ( target_climb_rate , takeoff_climb_rate ) ;
2015-04-30 03:06:55 -03:00
// check for take-off
2016-12-14 20:25:31 -04:00
# if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
2017-01-09 03:31:26 -04:00
if ( ap . land_complete & & ( takeoff_state . running | | ( target_climb_rate > 0.0f & & motors - > rotor_runup_complete ( ) ) ) ) {
2016-12-14 20:25:31 -04:00
# else
2016-08-04 03:04:59 -03:00
if ( ap . land_complete & & ( takeoff_state . running | | target_climb_rate > 0.0f ) ) {
2016-12-14 20:25:31 -04:00
# endif
2015-04-30 03:06:55 -03:00
if ( ! takeoff_state . running ) {
2015-04-30 02:03:59 -03:00
takeoff_timer_start ( constrain_float ( g . pilot_takeoff_alt , 0.0f , 1000.0f ) ) ;
2015-04-30 03:06:55 -03:00
}
2014-04-04 11:14:07 -03:00
// indicate we are taking off
set_land_complete ( false ) ;
// clear i term when we're taking off
set_throttle_takeoff ( ) ;
}
}
2014-08-29 03:25:37 -03:00
// relax loiter target if we might be landed
2015-05-06 01:38:38 -03:00
if ( ap . land_complete_maybe ) {
2018-03-27 23:13:37 -03:00
loiter_nav - > soften_for_landing ( ) ;
2014-08-29 03:25:37 -03:00
}
2014-04-05 11:20:39 -03:00
// if landed initialise loiter targets, set throttle to zero and exit
2014-04-04 11:14:07 -03:00
if ( ap . land_complete ) {
2016-07-31 23:12:31 -03:00
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if ( target_climb_rate < 0.0f ) {
2017-01-09 03:31:26 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2016-07-31 23:12:31 -03:00
} else {
2017-01-09 03:31:26 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2016-01-17 23:53:16 -04:00
}
2018-03-27 23:13:37 -03:00
loiter_nav - > init_target ( ) ;
2017-01-09 03:31:26 -04:00
attitude_control - > reset_rate_controller_I_terms ( ) ;
attitude_control - > set_yaw_target_to_current_heading ( ) ;
2017-06-26 05:48:04 -03:00
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( 0 , 0 , 0 ) ;
2017-01-09 03:31:26 -04:00
pos_control - > relax_alt_hold_controllers ( 0.0f ) ; // forces throttle output to go to zero
pos_control - > update_z_controller ( ) ;
2014-04-05 11:20:39 -03:00
return ;
2014-04-04 11:14:07 -03:00
} else {
// convert pilot input to lean angles
2018-03-18 08:06:54 -03:00
get_pilot_desired_lean_angles ( target_roll , target_pitch , copter . aparm . angle_max , attitude_control - > get_althold_lean_angle_max ( ) ) ;
2014-04-05 11:20:39 -03:00
2014-04-10 03:55:54 -03:00
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
2014-04-05 11:20:39 -03:00
vel_fw = vel . x * ahrs . cos_yaw ( ) + vel . y * ahrs . sin_yaw ( ) ;
vel_right = - vel . x * ahrs . sin_yaw ( ) + vel . y * ahrs . cos_yaw ( ) ;
2014-04-23 00:32:08 -03:00
2014-04-23 18:43:56 -03:00
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
2014-07-11 02:06:53 -03:00
if ( poshold . roll_mode ! = POSHOLD_LOITER | | poshold . pitch_mode ! = POSHOLD_LOITER )
poshold_get_wind_comp_lean_angles ( poshold . wind_comp_roll , poshold . wind_comp_pitch ) ;
2014-04-05 11:20:39 -03:00
2014-04-11 05:16:40 -03:00
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
2014-07-11 02:06:53 -03:00
switch ( poshold . roll_mode ) {
2014-04-05 11:20:39 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_PILOT_OVERRIDE :
2014-04-11 05:16:40 -03:00
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
2014-07-11 02:06:53 -03:00
poshold_update_pilot_lean_angle ( poshold . pilot_roll , target_roll ) ;
2014-04-11 05:16:40 -03:00
// switch to BRAKE mode for next iteration if no pilot input
2015-05-15 12:53:29 -03:00
if ( is_zero ( target_roll ) & & ( fabsf ( poshold . pilot_roll ) < 2 * g . poshold_brake_rate ) ) {
2014-04-11 05:16:40 -03:00
// initialise BRAKE mode
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_BRAKE ; // Set brake roll mode
poshold . brake_roll = 0 ; // initialise braking angle to zero
poshold . brake_angle_max_roll = 0 ; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold . brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX ; // number of cycles the brake will be applied, updated during braking mode.
poshold . braking_time_updated_roll = false ; // flag the braking time can be re-estimated
2014-04-04 11:14:07 -03:00
}
2014-04-05 11:20:39 -03:00
2014-04-11 05:16:40 -03:00
// final lean angle should be pilot input plus wind compensation
2014-07-11 02:06:53 -03:00
poshold . roll = poshold . pilot_roll + poshold . wind_comp_roll ;
2014-04-11 05:16:40 -03:00
break ;
2014-07-11 02:06:53 -03:00
case POSHOLD_BRAKE :
case POSHOLD_BRAKE_READY_TO_LOITER :
2014-04-11 05:16:40 -03:00
// calculate brake_roll angle to counter-act velocity
2014-07-11 02:06:53 -03:00
poshold_update_brake_angle_from_velocity ( poshold . brake_roll , vel_right ) ;
2014-04-11 05:16:40 -03:00
// update braking time estimate
2014-07-11 02:06:53 -03:00
if ( ! poshold . braking_time_updated_roll ) {
2014-04-11 05:16:40 -03:00
// check if brake angle is increasing
2014-07-11 02:06:53 -03:00
if ( abs ( poshold . brake_roll ) > = poshold . brake_angle_max_roll ) {
poshold . brake_angle_max_roll = abs ( poshold . brake_roll ) ;
2014-04-11 05:16:40 -03:00
} else {
// braking angle has started decreasing so re-estimate braking time
2014-07-11 02:06:53 -03:00
poshold . brake_timeout_roll = 1 + ( uint16_t ) ( LOOP_RATE_FACTOR * 15L * ( int32_t ) ( abs ( poshold . brake_roll ) ) / ( 10L * ( int32_t ) g . poshold_brake_rate ) ) ; // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold . braking_time_updated_roll = true ;
2014-04-11 05:16:40 -03:00
}
2014-04-04 11:14:07 -03:00
}
2014-04-05 11:20:39 -03:00
2014-04-11 05:16:40 -03:00
// if velocity is very low reduce braking time to 0.5seconds
2015-05-08 15:40:08 -03:00
if ( ( fabsf ( vel_right ) < = POSHOLD_SPEED_0 ) & & ( poshold . brake_timeout_roll > 50 * LOOP_RATE_FACTOR ) ) {
2014-07-11 02:06:53 -03:00
poshold . brake_timeout_roll = 50 * LOOP_RATE_FACTOR ;
2014-04-04 11:14:07 -03:00
}
2014-04-10 03:55:54 -03:00
2014-04-11 05:16:40 -03:00
// reduce braking timer
2014-07-11 02:06:53 -03:00
if ( poshold . brake_timeout_roll > 0 ) {
poshold . brake_timeout_roll - - ;
2014-04-10 03:55:54 -03:00
} else {
2014-04-11 05:16:40 -03:00
// indicate that we are ready to move to Loiter.
2014-07-11 02:06:53 -03:00
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
2014-04-11 05:16:40 -03:00
// logic for engaging loiter is handled below the roll and pitch mode switch statements
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
2014-04-04 11:14:07 -03:00
}
2014-04-23 00:39:46 -03:00
// final lean angle is braking angle + wind compensation angle
2014-07-11 02:06:53 -03:00
poshold . roll = poshold . brake_roll + poshold . wind_comp_roll ;
2014-04-23 00:39:46 -03:00
2014-04-11 05:16:40 -03:00
// check for pilot input
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_roll ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_roll_controller_to_pilot_override ( ) ;
2014-04-11 05:16:40 -03:00
}
2014-04-04 11:14:07 -03:00
break ;
2014-04-10 03:55:54 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_BRAKE_TO_LOITER :
case POSHOLD_LOITER :
2014-04-11 05:16:40 -03:00
// these modes are combined roll-pitch modes and are handled below
2014-04-04 11:14:07 -03:00
break ;
2014-04-10 03:55:54 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE :
2014-04-11 05:16:40 -03:00
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
2014-07-11 02:06:53 -03:00
poshold_update_pilot_lean_angle ( poshold . pilot_roll , target_roll ) ;
2014-04-11 05:16:40 -03:00
// count-down loiter to pilot timer
2014-07-11 02:06:53 -03:00
if ( poshold . controller_to_pilot_timer_roll > 0 ) {
poshold . controller_to_pilot_timer_roll - - ;
2014-04-11 05:16:40 -03:00
} else {
// when timer runs out switch to full pilot override for next iteration
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_PILOT_OVERRIDE ;
2014-04-11 05:16:40 -03:00
}
2014-04-23 00:39:46 -03:00
// calculate controller_to_pilot mix ratio
2014-07-11 02:06:53 -03:00
controller_to_pilot_roll_mix = ( float ) poshold . controller_to_pilot_timer_roll / ( float ) POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER ;
2014-04-11 05:16:40 -03:00
2014-04-13 01:47:47 -03:00
// mix final loiter lean angle and pilot desired lean angles
2014-07-11 02:06:53 -03:00
poshold . roll = poshold_mix_controls ( controller_to_pilot_roll_mix , poshold . controller_final_roll , poshold . pilot_roll + poshold . wind_comp_roll ) ;
2014-04-10 03:55:54 -03:00
break ;
2014-04-04 11:14:07 -03:00
}
2014-04-11 05:16:40 -03:00
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
2014-07-11 02:06:53 -03:00
switch ( poshold . pitch_mode ) {
2014-04-11 05:16:40 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_PILOT_OVERRIDE :
2014-04-11 05:16:40 -03:00
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
2014-07-11 02:06:53 -03:00
poshold_update_pilot_lean_angle ( poshold . pilot_pitch , target_pitch ) ;
2014-04-11 05:16:40 -03:00
// switch to BRAKE mode for next iteration if no pilot input
2015-05-15 12:53:29 -03:00
if ( is_zero ( target_pitch ) & & ( fabsf ( poshold . pilot_pitch ) < 2 * g . poshold_brake_rate ) ) {
2014-04-11 05:16:40 -03:00
// initialise BRAKE mode
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = POSHOLD_BRAKE ; // set brake pitch mode
poshold . brake_pitch = 0 ; // initialise braking angle to zero
poshold . brake_angle_max_pitch = 0 ; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold . brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX ; // number of cycles the brake will be applied, updated during braking mode.
poshold . braking_time_updated_pitch = false ; // flag the braking time can be re-estimated
2014-04-11 05:16:40 -03:00
}
// final lean angle should be pilot input plus wind compensation
2014-07-11 02:06:53 -03:00
poshold . pitch = poshold . pilot_pitch + poshold . wind_comp_pitch ;
2014-04-04 11:14:07 -03:00
break ;
2014-04-10 03:55:54 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_BRAKE :
case POSHOLD_BRAKE_READY_TO_LOITER :
2014-04-11 05:16:40 -03:00
// calculate brake_pitch angle to counter-act velocity
2014-07-11 02:06:53 -03:00
poshold_update_brake_angle_from_velocity ( poshold . brake_pitch , - vel_fw ) ;
2014-04-11 05:16:40 -03:00
// update braking time estimate
2014-07-11 02:06:53 -03:00
if ( ! poshold . braking_time_updated_pitch ) {
2014-04-11 05:16:40 -03:00
// check if brake angle is increasing
2014-07-11 02:06:53 -03:00
if ( abs ( poshold . brake_pitch ) > = poshold . brake_angle_max_pitch ) {
poshold . brake_angle_max_pitch = abs ( poshold . brake_pitch ) ;
2014-04-11 05:16:40 -03:00
} else {
// braking angle has started decreasing so re-estimate braking time
2014-07-11 02:06:53 -03:00
poshold . brake_timeout_pitch = 1 + ( uint16_t ) ( LOOP_RATE_FACTOR * 15L * ( int32_t ) ( abs ( poshold . brake_pitch ) ) / ( 10L * ( int32_t ) g . poshold_brake_rate ) ) ; // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold . braking_time_updated_pitch = true ;
2014-04-11 05:16:40 -03:00
}
}
// if velocity is very low reduce braking time to 0.5seconds
2015-05-08 15:40:08 -03:00
if ( ( fabsf ( vel_fw ) < = POSHOLD_SPEED_0 ) & & ( poshold . brake_timeout_pitch > 50 * LOOP_RATE_FACTOR ) ) {
2014-07-11 02:06:53 -03:00
poshold . brake_timeout_pitch = 50 * LOOP_RATE_FACTOR ;
2014-04-11 05:16:40 -03:00
}
// reduce braking timer
2014-07-11 02:06:53 -03:00
if ( poshold . brake_timeout_pitch > 0 ) {
poshold . brake_timeout_pitch - - ;
2014-04-11 05:16:40 -03:00
} else {
// indicate that we are ready to move to Loiter.
2014-07-11 02:06:53 -03:00
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
2014-04-11 05:16:40 -03:00
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
2014-04-11 05:16:40 -03:00
}
2014-04-23 00:39:46 -03:00
// final lean angle is braking angle + wind compensation angle
2014-07-11 02:06:53 -03:00
poshold . pitch = poshold . brake_pitch + poshold . wind_comp_pitch ;
2014-04-23 00:39:46 -03:00
2014-04-11 05:16:40 -03:00
// check for pilot input
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_pitch ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_pitch_controller_to_pilot_override ( ) ;
2014-04-11 05:16:40 -03:00
}
2014-04-04 11:14:07 -03:00
break ;
2014-04-10 03:55:54 -03:00
2014-07-11 02:06:53 -03:00
case POSHOLD_BRAKE_TO_LOITER :
case POSHOLD_LOITER :
2014-04-11 05:16:40 -03:00
// these modes are combined pitch-pitch modes and are handled below
2014-04-10 03:55:54 -03:00
break ;
2014-07-11 02:06:53 -03:00
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE :
2014-04-11 05:16:40 -03:00
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
2014-07-11 02:06:53 -03:00
poshold_update_pilot_lean_angle ( poshold . pilot_pitch , target_pitch ) ;
2014-04-11 05:16:40 -03:00
// count-down loiter to pilot timer
2014-07-11 02:06:53 -03:00
if ( poshold . controller_to_pilot_timer_pitch > 0 ) {
poshold . controller_to_pilot_timer_pitch - - ;
2014-04-04 11:14:07 -03:00
} else {
2014-04-11 05:16:40 -03:00
// when timer runs out switch to full pilot override for next iteration
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = POSHOLD_PILOT_OVERRIDE ;
2014-04-04 11:14:07 -03:00
}
2014-04-10 03:55:54 -03:00
2014-04-23 00:39:46 -03:00
// calculate controller_to_pilot mix ratio
2014-07-11 02:06:53 -03:00
controller_to_pilot_pitch_mix = ( float ) poshold . controller_to_pilot_timer_pitch / ( float ) POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER ;
2014-04-11 05:16:40 -03:00
2014-04-13 01:47:47 -03:00
// mix final loiter lean angle and pilot desired lean angles
2014-07-11 02:06:53 -03:00
poshold . pitch = poshold_mix_controls ( controller_to_pilot_pitch_mix , poshold . controller_final_pitch , poshold . pilot_pitch + poshold . wind_comp_pitch ) ;
2014-04-10 03:55:54 -03:00
break ;
2014-04-04 11:14:07 -03:00
}
2014-04-05 11:20:39 -03:00
2016-01-17 23:53:16 -04:00
// set motors to full range
2017-01-09 03:31:26 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2016-01-17 23:53:16 -04:00
2014-04-11 05:16:40 -03:00
//
2014-07-11 02:06:53 -03:00
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
2014-04-11 05:16:40 -03:00
//
// switch into LOITER mode when both roll and pitch are ready
2014-07-11 02:06:53 -03:00
if ( poshold . roll_mode = = POSHOLD_BRAKE_READY_TO_LOITER & & poshold . pitch_mode = = POSHOLD_BRAKE_READY_TO_LOITER ) {
poshold . roll_mode = POSHOLD_BRAKE_TO_LOITER ;
poshold . pitch_mode = POSHOLD_BRAKE_TO_LOITER ;
poshold . brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER ;
2014-04-11 05:16:40 -03:00
// init loiter controller
2018-03-27 23:13:37 -03:00
loiter_nav - > init_target ( inertial_nav . get_position ( ) ) ;
2014-04-13 02:33:31 -03:00
// set delay to start of wind compensation estimate updates
2014-07-11 02:06:53 -03:00
poshold . wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER ;
2014-04-11 05:16:40 -03:00
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
2014-07-11 02:06:53 -03:00
if ( poshold . roll_mode = = POSHOLD_BRAKE_TO_LOITER | | poshold . roll_mode = = POSHOLD_LOITER ) {
2014-04-11 05:16:40 -03:00
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = poshold . roll_mode ;
2014-04-11 05:16:40 -03:00
// handle combined roll+pitch mode
2014-07-11 02:06:53 -03:00
switch ( poshold . roll_mode ) {
case POSHOLD_BRAKE_TO_LOITER :
2014-04-11 05:16:40 -03:00
// reduce brake_to_loiter timer
2014-07-11 02:06:53 -03:00
if ( poshold . brake_to_loiter_timer > 0 ) {
poshold . brake_to_loiter_timer - - ;
2014-04-11 05:16:40 -03:00
} else {
// progress to full loiter on next iteration
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_LOITER ;
poshold . pitch_mode = POSHOLD_LOITER ;
2014-04-11 05:16:40 -03:00
}
// calculate percentage mix of loiter and brake control
2014-07-11 02:06:53 -03:00
brake_to_loiter_mix = ( float ) poshold . brake_to_loiter_timer / ( float ) POSHOLD_BRAKE_TO_LOITER_TIMER ;
2014-04-11 05:16:40 -03:00
// calculate brake_roll and pitch angles to counter-act velocity
2014-07-11 02:06:53 -03:00
poshold_update_brake_angle_from_velocity ( poshold . brake_roll , vel_right ) ;
poshold_update_brake_angle_from_velocity ( poshold . brake_pitch , - vel_fw ) ;
2014-04-11 05:16:40 -03:00
// run loiter controller
2018-03-27 23:13:37 -03:00
loiter_nav - > update ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2014-04-11 05:16:40 -03:00
// calculate final roll and pitch output by mixing loiter and brake controls
2018-03-27 23:13:37 -03:00
poshold . roll = poshold_mix_controls ( brake_to_loiter_mix , poshold . brake_roll + poshold . wind_comp_roll , loiter_nav - > get_roll ( ) ) ;
poshold . pitch = poshold_mix_controls ( brake_to_loiter_mix , poshold . brake_pitch + poshold . wind_comp_pitch , loiter_nav - > get_pitch ( ) ) ;
2014-04-11 05:16:40 -03:00
// check for pilot input
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_roll ) | | ! is_zero ( target_pitch ) ) {
2014-04-11 05:16:40 -03:00
// if roll input switch to pilot override for roll
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_roll ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_roll_controller_to_pilot_override ( ) ;
2014-04-11 05:16:40 -03:00
// switch pitch-mode to brake (but ready to go back to loiter anytime)
2014-07-11 02:06:53 -03:00
// no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation
poshold . pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
2014-04-11 05:16:40 -03:00
}
// if pitch input switch to pilot override for pitch
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_pitch ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_pitch_controller_to_pilot_override ( ) ;
2015-05-04 23:34:21 -03:00
if ( is_zero ( target_roll ) ) {
2014-04-11 05:16:40 -03:00
// switch roll-mode to brake (but ready to go back to loiter anytime)
2014-07-11 02:06:53 -03:00
// no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation
poshold . roll_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
2014-04-11 05:16:40 -03:00
}
}
}
break ;
2014-07-11 02:06:53 -03:00
case POSHOLD_LOITER :
2014-04-11 05:16:40 -03:00
// run loiter controller
2018-03-27 23:13:37 -03:00
loiter_nav - > update ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2014-04-11 05:16:40 -03:00
// set roll angle based on loiter controller outputs
2018-03-27 23:13:37 -03:00
poshold . roll = loiter_nav - > get_roll ( ) ;
poshold . pitch = loiter_nav - > get_pitch ( ) ;
2014-04-11 05:16:40 -03:00
// update wind compensation estimate
2014-07-11 02:06:53 -03:00
poshold_update_wind_comp_estimate ( ) ;
2014-04-11 05:16:40 -03:00
// check for pilot input
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_roll ) | | ! is_zero ( target_pitch ) ) {
2014-04-11 05:16:40 -03:00
// if roll input switch to pilot override for roll
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_roll ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_roll_controller_to_pilot_override ( ) ;
2014-04-11 05:16:40 -03:00
// switch pitch-mode to brake (but ready to go back to loiter anytime)
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
Copter: Hybrid fixes to wind_comp, brake pitch timer, thr peaks
There was an error in the velocity axis used to update
brake_timeout_pitch (vel_right instead of vel_fw)
The wind_comp was not enough filtered for the Pixhawk (400Hz), so I
added a specific time constant (TC_WIND_COMP) to have the expected
filter with 400Hz controllers.
About throttle peaks, after some tests and from logs, they happen when
hybrid switches to loiter.
There is always a difference between Alt and DesiredAlt (DAlt), but,
when loiter engages, it initializes DAlt = Alt and the copter tries
immediatelly to reach that new setpoint. So the solution would be to
init_loiter_target() just as it was in pre-onion code : only x/y and not
z. and to be able to pass parameters like that
wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
Well, from this new code structure, it seems not possible with current
functions so I've used set_loiter_target that init position passed as
parameter and velocity to 0 (as expected).
BTW, I think there was something wrong with set_loiter_target function,
the "Vector3f& position" parameter was not used at all...
I moved the reset flag from init_loiter_target to set_loiter_target.
2014-04-23 01:40:03 -03:00
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
2014-07-11 02:06:53 -03:00
poshold . brake_pitch = 0 ;
2014-04-11 05:16:40 -03:00
}
// if pitch input switch to pilot override for pitch
2015-05-04 23:34:21 -03:00
if ( ! is_zero ( target_pitch ) ) {
2014-04-23 00:39:46 -03:00
// init transition to pilot override
2014-07-11 02:06:53 -03:00
poshold_pitch_controller_to_pilot_override ( ) ;
2014-04-11 05:16:40 -03:00
// if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
2015-05-04 23:34:21 -03:00
if ( is_zero ( target_roll ) ) {
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_BRAKE_READY_TO_LOITER ;
poshold . brake_roll = 0 ;
2014-04-11 05:16:40 -03:00
}
}
}
break ;
default :
// do nothing for uncombined roll and pitch modes
break ;
}
}
2014-04-23 00:32:08 -03:00
2014-04-05 11:20:39 -03:00
// constrain target pitch/roll angles
2018-02-07 22:21:09 -04:00
float angle_max = copter . aparm . angle_max ;
2016-03-22 21:51:39 -03:00
poshold . roll = constrain_int16 ( poshold . roll , - angle_max , angle_max ) ;
poshold . pitch = constrain_int16 ( poshold . pitch , - angle_max , angle_max ) ;
2014-04-05 11:20:39 -03:00
// update attitude controller targets
2017-06-26 05:48:04 -03:00
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( poshold . roll , poshold . pitch , target_yaw_rate ) ;
2014-04-04 11:14:07 -03:00
2016-04-27 09:18:35 -03:00
// adjust climb rate using rangefinder
2018-06-05 07:58:21 -03:00
target_climb_rate = get_surface_tracking_climb_rate ( target_climb_rate , pos_control - > get_alt_target ( ) , G_Dt ) ;
2017-01-05 02:24:02 -04:00
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate ( target_climb_rate ) ;
2014-04-04 11:14:07 -03:00
// update altitude target and call position controller
2017-01-09 03:31:26 -04:00
pos_control - > set_alt_target_from_climb_rate_ff ( target_climb_rate , G_Dt , false ) ;
pos_control - > add_takeoff_climb_rate ( takeoff_climb_rate , G_Dt ) ;
pos_control - > update_z_controller ( ) ;
2014-04-04 11:14:07 -03:00
}
2014-04-10 03:55:54 -03:00
}
2014-07-11 02:06:53 -03:00
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_update_pilot_lean_angle ( float & lean_angle_filtered , float & lean_angle_raw )
2014-04-11 05:16:40 -03:00
{
// if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
2015-05-15 12:53:29 -03:00
if ( ( lean_angle_filtered > 0 & & lean_angle_raw < 0 ) | | ( lean_angle_filtered < 0 & & lean_angle_raw > 0 ) | | ( fabsf ( lean_angle_raw ) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE ) ) {
2014-04-11 05:16:40 -03:00
lean_angle_filtered = lean_angle_raw ;
} else {
// lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
if ( lean_angle_filtered > 0 ) {
2014-04-23 00:32:08 -03:00
// reduce the filtered lean angle at 5% or the brake rate (whichever is faster).
2015-11-27 13:11:58 -04:00
lean_angle_filtered - = MAX ( ( float ) lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR , MAX ( 1 , g . poshold_brake_rate / LOOP_RATE_FACTOR ) ) ;
2014-04-11 05:16:40 -03:00
// do not let the filtered angle fall below the pilot's input lean angle.
// the above line pulls the filtered angle down and the below line acts as a catch
2015-11-27 13:11:58 -04:00
lean_angle_filtered = MAX ( lean_angle_filtered , lean_angle_raw ) ;
2014-04-11 05:16:40 -03:00
} else {
2015-11-27 13:11:58 -04:00
lean_angle_filtered + = MAX ( - ( float ) lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR , MAX ( 1 , g . poshold_brake_rate / LOOP_RATE_FACTOR ) ) ;
lean_angle_filtered = MIN ( lean_angle_filtered , lean_angle_raw ) ;
2014-04-11 05:16:40 -03:00
}
}
}
2014-07-11 02:06:53 -03:00
// poshold_mix_controls - mixes two controls based on the mix_ratio
2014-04-13 01:47:47 -03:00
// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
2017-12-10 23:51:13 -04:00
int16_t Copter : : ModePosHold : : poshold_mix_controls ( float mix_ratio , int16_t first_control , int16_t second_control )
2014-04-13 01:47:47 -03:00
{
mix_ratio = constrain_float ( mix_ratio , 0.0f , 1.0f ) ;
return ( int16_t ) ( ( mix_ratio * first_control ) + ( ( 1.0f - mix_ratio ) * second_control ) ) ;
}
2014-07-11 02:06:53 -03:00
// poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain
// brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max
2014-04-11 05:16:40 -03:00
// velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity)
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_update_brake_angle_from_velocity ( int16_t & brake_angle , float velocity )
2014-04-11 05:16:40 -03:00
{
float lean_angle ;
2014-07-11 02:06:53 -03:00
int16_t brake_rate = g . poshold_brake_rate ;
2014-04-13 02:33:31 -03:00
brake_rate / = 4 ;
if ( brake_rate < = 0 ) {
brake_rate = 1 ;
}
2014-04-11 05:16:40 -03:00
// calculate velocity-only based lean angle
if ( velocity > = 0 ) {
2014-07-11 02:06:53 -03:00
lean_angle = - poshold . brake_gain * velocity * ( 1.0f + 500.0f / ( velocity + 60.0f ) ) ;
2014-04-11 05:16:40 -03:00
} else {
2014-07-11 02:06:53 -03:00
lean_angle = - poshold . brake_gain * velocity * ( 1.0f + 500.0f / ( - velocity + 60.0f ) ) ;
2014-04-11 05:16:40 -03:00
}
// do not let lean_angle be too far from brake_angle
2014-04-13 02:33:31 -03:00
brake_angle = constrain_int16 ( ( int16_t ) lean_angle , brake_angle - brake_rate , brake_angle + brake_rate ) ;
2014-04-11 05:16:40 -03:00
// constrain final brake_angle
2014-07-11 02:06:53 -03:00
brake_angle = constrain_int16 ( brake_angle , - g . poshold_brake_angle_max , g . poshold_brake_angle_max ) ;
2014-04-11 05:16:40 -03:00
}
2014-07-11 02:06:53 -03:00
// poshold_update_wind_comp_estimate - updates wind compensation estimate
2014-04-10 03:55:54 -03:00
// should be called at the maximum loop rate when loiter is engaged
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_update_wind_comp_estimate ( )
2014-04-10 03:55:54 -03:00
{
2014-04-13 02:33:31 -03:00
// check wind estimate start has not been delayed
2014-07-11 02:06:53 -03:00
if ( poshold . wind_comp_start_timer > 0 ) {
poshold . wind_comp_start_timer - - ;
2014-04-13 02:33:31 -03:00
return ;
}
// check horizontal velocity is low
2014-07-11 02:06:53 -03:00
if ( inertial_nav . get_velocity_xy ( ) > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX ) {
2014-04-10 03:55:54 -03:00
return ;
}
2014-04-11 05:16:40 -03:00
// get position controller accel target
2014-07-11 02:06:53 -03:00
// To-Do: clean this up by using accessor in loiter controller (or move entire PosHold controller to a library shared with loiter)
2017-01-09 03:31:26 -04:00
const Vector3f & accel_target = pos_control - > get_accel_target ( ) ;
2014-04-11 05:16:40 -03:00
2014-04-10 03:55:54 -03:00
// update wind compensation in earth-frame lean angles
2015-05-04 23:34:21 -03:00
if ( is_zero ( poshold . wind_comp_ef . x ) ) {
2014-04-10 03:55:54 -03:00
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
2014-07-11 02:06:53 -03:00
poshold . wind_comp_ef . x = accel_target . x ;
2014-04-10 03:55:54 -03:00
} else {
// low pass filter the position controller's lean angle output
2014-07-11 02:06:53 -03:00
poshold . wind_comp_ef . x = ( 1.0f - TC_WIND_COMP ) * poshold . wind_comp_ef . x + TC_WIND_COMP * accel_target . x ;
2014-04-10 03:55:54 -03:00
}
2015-05-04 23:34:21 -03:00
if ( is_zero ( poshold . wind_comp_ef . y ) ) {
2014-04-10 03:55:54 -03:00
// if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction
2014-07-11 02:06:53 -03:00
poshold . wind_comp_ef . y = accel_target . y ;
2014-04-10 03:55:54 -03:00
} else {
// low pass filter the position controller's lean angle output
2014-07-11 02:06:53 -03:00
poshold . wind_comp_ef . y = ( 1.0f - TC_WIND_COMP ) * poshold . wind_comp_ef . y + TC_WIND_COMP * accel_target . y ;
2014-04-10 03:55:54 -03:00
}
}
2014-07-11 02:06:53 -03:00
// poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles
2014-04-16 09:18:01 -03:00
// should be called at the maximum loop rate
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_get_wind_comp_lean_angles ( int16_t & roll_angle , int16_t & pitch_angle )
2014-04-10 03:55:54 -03:00
{
2014-04-16 09:18:01 -03:00
// reduce rate to 10hz
2014-07-11 02:06:53 -03:00
poshold . wind_comp_timer + + ;
if ( poshold . wind_comp_timer < POSHOLD_WIND_COMP_TIMER_10HZ ) {
2014-04-16 09:18:01 -03:00
return ;
}
2014-07-11 02:06:53 -03:00
poshold . wind_comp_timer = 0 ;
2014-04-16 09:18:01 -03:00
2014-04-10 03:55:54 -03:00
// convert earth frame desired accelerations to body frame roll and pitch lean angles
2016-02-25 13:13:02 -04:00
roll_angle = atanf ( ( - poshold . wind_comp_ef . x * ahrs . sin_yaw ( ) + poshold . wind_comp_ef . y * ahrs . cos_yaw ( ) ) / 981 ) * ( 18000 / M_PI ) ;
pitch_angle = atanf ( - ( poshold . wind_comp_ef . x * ahrs . cos_yaw ( ) + poshold . wind_comp_ef . y * ahrs . sin_yaw ( ) ) / 981 ) * ( 18000 / M_PI ) ;
2014-04-04 11:14:07 -03:00
}
2014-04-23 00:39:46 -03:00
2014-07-11 02:06:53 -03:00
// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_roll_controller_to_pilot_override ( )
2014-04-23 00:39:46 -03:00
{
2014-07-11 02:06:53 -03:00
poshold . roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE ;
poshold . controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER ;
// initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
poshold . pilot_roll = 0 ;
2014-04-23 00:39:46 -03:00
// store final controller output for mixing with pilot input
2014-07-11 02:06:53 -03:00
poshold . controller_final_roll = poshold . roll ;
2014-04-23 00:39:46 -03:00
}
2014-07-11 02:06:53 -03:00
// poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
2017-12-10 23:51:13 -04:00
void Copter : : ModePosHold : : poshold_pitch_controller_to_pilot_override ( )
2014-04-23 00:39:46 -03:00
{
2014-07-11 02:06:53 -03:00
poshold . pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE ;
poshold . controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER ;
// initialise pilot_pitch to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value
poshold . pilot_pitch = 0 ;
2014-04-23 00:39:46 -03:00
// store final loiter outputs for mixing with pilot input
2014-07-11 02:06:53 -03:00
poshold . controller_final_pitch = poshold . pitch ;
2014-04-23 00:39:46 -03:00
}