2011-03-19 07:20:11 -03:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2011-12-03 21:54:38 -04:00
|
|
|
|
2014-02-12 03:28:41 -04:00
|
|
|
// local variables
|
|
|
|
float roll_in_filtered; // roll-in in filtered with RC_FEEL_RP parameter
|
|
|
|
float pitch_in_filtered; // pitch-in filtered with RC_FEEL_RP parameter
|
|
|
|
|
|
|
|
static void reset_roll_pitch_in_filters(int16_t roll_in, int16_t pitch_in)
|
|
|
|
{
|
|
|
|
roll_in_filtered = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
|
|
|
|
pitch_in_filtered = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
|
|
|
|
}
|
|
|
|
|
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
|
|
|
|
static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
|
|
|
|
{
|
|
|
|
static float _scaler = 1.0;
|
|
|
|
static int16_t _angle_max = 0;
|
|
|
|
|
2013-11-13 01:21:21 -04:00
|
|
|
// range check the input
|
|
|
|
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
|
|
|
|
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
|
|
|
|
|
2014-02-12 03:28:41 -04:00
|
|
|
// filter input for feel
|
|
|
|
if (g.rc_feel_rp >= RC_FEEL_RP_VERY_CRISP) {
|
|
|
|
// no filtering required
|
|
|
|
roll_in_filtered = roll_in;
|
|
|
|
pitch_in_filtered = pitch_in;
|
|
|
|
}else{
|
|
|
|
float filter_gain;
|
|
|
|
if (g.rc_feel_rp >= RC_FEEL_RP_CRISP) {
|
|
|
|
filter_gain = 0.5;
|
|
|
|
} else if(g.rc_feel_rp >= RC_FEEL_RP_MEDIUM) {
|
|
|
|
filter_gain = 0.3;
|
|
|
|
} else if(g.rc_feel_rp >= RC_FEEL_RP_SOFT) {
|
|
|
|
filter_gain = 0.05;
|
|
|
|
} else {
|
|
|
|
// must be RC_FEEL_RP_VERY_SOFT
|
|
|
|
filter_gain = 0.02;
|
|
|
|
}
|
|
|
|
roll_in_filtered = roll_in_filtered * (1.0 - filter_gain) + (float)roll_in * filter_gain;
|
|
|
|
pitch_in_filtered = pitch_in_filtered * (1.0 - filter_gain) + (float)pitch_in * filter_gain;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return filtered roll if no scaling required
|
2013-10-18 05:03:31 -03:00
|
|
|
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {
|
2014-02-12 03:28:41 -04:00
|
|
|
roll_out = (int16_t)roll_in_filtered;
|
|
|
|
pitch_out = (int16_t)pitch_in_filtered;
|
2013-08-11 00:51:08 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if angle_max has been updated and redo scaler
|
2013-10-18 05:03:31 -03:00
|
|
|
if (aparm.angle_max != _angle_max) {
|
|
|
|
_angle_max = aparm.angle_max;
|
|
|
|
_scaler = (float)aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
|
2013-08-11 00:51:08 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// convert pilot input to lean angle
|
2014-02-12 03:28:41 -04:00
|
|
|
roll_out = (int16_t)(roll_in_filtered * _scaler);
|
|
|
|
pitch_out = (int16_t)(pitch_in_filtered * _scaler);
|
2013-08-11 00:51:08 -03:00
|
|
|
}
|
|
|
|
|
2013-12-06 02:08:11 -04:00
|
|
|
// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
|
|
|
|
// returns desired angle in centi-degrees
|
|
|
|
// To-Do: return heading as a float?
|
|
|
|
static float get_pilot_desired_yaw_rate(int16_t stick_angle)
|
|
|
|
{
|
|
|
|
// convert pilot input to the desired yaw rate
|
|
|
|
return stick_angle * g.acro_yaw_p;
|
|
|
|
}
|
|
|
|
|
2012-12-08 01:23:32 -04:00
|
|
|
/*************************************************************
|
|
|
|
* yaw controllers
|
|
|
|
*************************************************************/
|
|
|
|
|
2013-03-22 05:38:07 -03:00
|
|
|
// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller
|
|
|
|
// should be called at 100hz
|
2014-01-23 01:16:06 -04:00
|
|
|
static float get_look_at_yaw()
|
2013-03-22 05:38:07 -03:00
|
|
|
{
|
|
|
|
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz
|
|
|
|
|
|
|
|
look_at_yaw_counter++;
|
2014-01-23 01:16:06 -04:00
|
|
|
if (look_at_yaw_counter >= 10) {
|
2013-03-22 05:38:07 -03:00
|
|
|
look_at_yaw_counter = 0;
|
|
|
|
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
|
|
|
|
}
|
|
|
|
|
2014-01-23 01:16:06 -04:00
|
|
|
return yaw_look_at_WP_bearing;
|
2013-03-22 05:38:07 -03:00
|
|
|
}
|
|
|
|
|
2014-01-23 01:16:06 -04:00
|
|
|
static float get_look_ahead_yaw()
|
2012-12-08 01:23:32 -04:00
|
|
|
{
|
|
|
|
// Commanded Yaw to automatically look ahead.
|
2013-08-01 02:12:55 -03:00
|
|
|
if (g_gps->fix && g_gps->ground_speed_cm > YAW_LOOK_AHEAD_MIN_SPEED) {
|
2014-01-23 01:16:06 -04:00
|
|
|
yaw_look_ahead_bearing = g_gps->ground_course_cd;
|
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
|
|
|
|
****************************************************************/
|
|
|
|
|
2014-02-03 01:06:08 -04:00
|
|
|
// update_thr_cruise - update throttle cruise if necessary
|
|
|
|
// should be called at 100hz
|
|
|
|
static void update_thr_cruise()
|
2012-11-23 02:57:49 -04:00
|
|
|
{
|
|
|
|
// ensure throttle_avg has been initialised
|
|
|
|
if( throttle_avg == 0 ) {
|
|
|
|
throttle_avg = g.throttle_cruise;
|
2014-02-03 01:06:08 -04:00
|
|
|
// update position controller
|
|
|
|
pos_control.set_throttle_hover(throttle_avg);
|
|
|
|
}
|
|
|
|
|
|
|
|
// if not armed or landed exit
|
|
|
|
if (!motors.armed() || ap.land_complete) {
|
|
|
|
return;
|
2012-11-23 02:57:49 -04:00
|
|
|
}
|
2014-02-03 01:06:08 -04:00
|
|
|
|
|
|
|
// get throttle output
|
|
|
|
int16_t throttle = g.rc_3.servo_out;
|
|
|
|
|
2012-11-24 09:50:09 -04:00
|
|
|
// calc average throttle if we are in a level hover
|
|
|
|
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
|
2013-01-10 14:42:24 -04:00
|
|
|
throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f;
|
2012-11-23 02:57:49 -04:00
|
|
|
g.throttle_cruise = throttle_avg;
|
2013-12-29 04:26:39 -04:00
|
|
|
// update position controller
|
|
|
|
pos_control.set_throttle_hover(throttle_avg);
|
2012-11-24 00:41:17 -04:00
|
|
|
}
|
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
|
|
|
|
static void
|
|
|
|
set_throttle_takeoff()
|
|
|
|
{
|
2013-12-30 09:13:27 -04:00
|
|
|
// tell position controller to reset alt target and reset I terms
|
|
|
|
pos_control.init_takeoff();
|
2013-11-11 09:29:09 -04:00
|
|
|
|
2013-09-12 10:29:53 -03:00
|
|
|
// tell motors to do a slow start
|
|
|
|
motors.slow_start(true);
|
2013-07-26 09:43:09 -03:00
|
|
|
}
|
|
|
|
|
2013-01-30 11:25:41 -04:00
|
|
|
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
|
|
|
|
// used only for manual throttle modes
|
|
|
|
// returns throttle output 0 to 1000
|
|
|
|
#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
|
|
|
|
static int16_t get_pilot_desired_throttle(int16_t throttle_control)
|
|
|
|
{
|
|
|
|
int16_t throttle_out;
|
|
|
|
|
|
|
|
// exit immediately in the simple cases
|
|
|
|
if( throttle_control == 0 || g.throttle_mid == 500) {
|
|
|
|
return throttle_control;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure reasonable throttle values
|
2013-04-21 09:52:30 -03:00
|
|
|
throttle_control = constrain_int16(throttle_control,0,1000);
|
|
|
|
g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
|
2013-01-30 11:25:41 -04:00
|
|
|
|
|
|
|
// check throttle is above, below or in the deadband
|
|
|
|
if (throttle_control < THROTTLE_IN_MIDDLE) {
|
|
|
|
// below the deadband
|
2013-01-31 10:21:18 -04:00
|
|
|
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min));
|
2013-01-30 11:25:41 -04:00
|
|
|
}else if(throttle_control > THROTTLE_IN_MIDDLE) {
|
|
|
|
// above the deadband
|
|
|
|
throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f;
|
|
|
|
}else{
|
|
|
|
// must be in the deadband
|
|
|
|
throttle_out = g.throttle_mid;
|
|
|
|
}
|
|
|
|
|
|
|
|
return throttle_out;
|
|
|
|
}
|
|
|
|
|
2012-11-23 02:57:49 -04:00
|
|
|
// get_pilot_desired_climb_rate - transform pilot's throttle input to
|
|
|
|
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
|
|
|
|
// without any deadzone at the bottom
|
|
|
|
#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband
|
|
|
|
#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband
|
|
|
|
static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
|
|
|
|
{
|
|
|
|
int16_t desired_rate = 0;
|
|
|
|
|
|
|
|
// throttle failsafe check
|
2013-09-26 05:54:33 -03:00
|
|
|
if( failsafe.radio ) {
|
2012-11-23 02:57:49 -04:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure a reasonable throttle value
|
2013-04-21 09:52:30 -03:00
|
|
|
throttle_control = constrain_int16(throttle_control,0,1000);
|
2012-11-23 02:57:49 -04:00
|
|
|
|
|
|
|
// check throttle is above, below or in the deadband
|
|
|
|
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
|
|
|
|
// below the deadband
|
2012-12-21 23:52:49 -04:00
|
|
|
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
|
2012-11-23 02:57:49 -04:00
|
|
|
}else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
|
|
|
|
// above the deadband
|
2012-12-21 23:52:49 -04:00
|
|
|
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
|
2012-11-23 02:57:49 -04:00
|
|
|
}else{
|
|
|
|
// must be in the deadband
|
|
|
|
desired_rate = 0;
|
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2012-12-29 00:51:14 -04:00
|
|
|
// get_throttle_surface_tracking - 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
|
2014-02-03 03:22:59 -04:00
|
|
|
static float get_throttle_surface_tracking(int16_t target_rate, float current_alt_target, float dt)
|
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;
|
2012-12-29 00:51:14 -04:00
|
|
|
|
|
|
|
uint32_t now = millis();
|
|
|
|
|
|
|
|
// reset target altitude if this controller has just been engaged
|
2013-01-08 03:41:07 -04:00
|
|
|
if( now - last_call_ms > 200 ) {
|
2014-02-03 03:22:59 -04:00
|
|
|
target_sonar_alt = sonar_alt + current_alt_target - current_loc.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
|
|
|
|
2013-08-18 21:52:59 -03:00
|
|
|
// adjust sonar target alt if motors have not hit their limits
|
2013-07-25 12:45:59 -03:00
|
|
|
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
|
2014-01-23 23:30:26 -04:00
|
|
|
target_sonar_alt += target_rate * dt;
|
2013-07-25 12:45:59 -03:00
|
|
|
}
|
2012-12-29 00:51:14 -04:00
|
|
|
|
|
|
|
// do not let target altitude get too far from current altitude above ground
|
|
|
|
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
|
2014-01-24 02:49:15 -04:00
|
|
|
target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-pos_control.get_leash_down_z(),sonar_alt+pos_control.get_leash_up_z());
|
2013-04-08 23:58:01 -03:00
|
|
|
|
2013-08-18 21:52:59 -03:00
|
|
|
// calc desired velocity correction from target sonar alt vs actual sonar alt
|
|
|
|
distance_error = target_sonar_alt-sonar_alt;
|
|
|
|
velocity_correction = distance_error * g.sonar_gain;
|
|
|
|
velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
|
2012-12-29 00:51:14 -04:00
|
|
|
|
2014-01-23 23:30:26 -04:00
|
|
|
// return combined pilot climb rate + rate to correct sonar alt error
|
|
|
|
return (target_rate + velocity_correction);
|
2012-12-29 00:51:14 -04:00
|
|
|
}
|
|
|
|
|
2014-02-11 09:33:08 -04:00
|
|
|
// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
|
2013-01-31 03:30:03 -04:00
|
|
|
static void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle)
|
2013-01-11 23:20:37 -04:00
|
|
|
{
|
|
|
|
// shift difference between pilot's throttle and hover throttle into accelerometer I
|
2013-01-31 03:30:03 -04:00
|
|
|
g.pid_throttle_accel.set_integrator(pilot_throttle-g.throttle_cruise);
|
2013-01-11 23:20:37 -04:00
|
|
|
}
|