2011-03-19 07:20:11 -03:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-07-10 21:47:08 -03:00
|
|
|
get_stabilize_roll(long target_angle)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-07-10 21:47:08 -03:00
|
|
|
long error;
|
|
|
|
long rate;
|
2011-01-25 01:53:36 -04:00
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
error = wrap_180(target_angle - dcm.roll_sensor);
|
2011-01-25 01:53:36 -04:00
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
// limit the error we're feeding to the PID
|
|
|
|
error = constrain(error, -2500, 2500);
|
|
|
|
|
|
|
|
// desired Rate:
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
|
|
|
|
|
2011-08-13 06:37:01 -03:00
|
|
|
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
2011-07-10 21:47:08 -03:00
|
|
|
// Rate P:
|
|
|
|
error = rate - (long)(degrees(omega.x) * 100.0);
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
2011-08-13 06:37:01 -03:00
|
|
|
#endif
|
2011-07-10 21:47:08 -03:00
|
|
|
|
|
|
|
// output control:
|
2011-09-04 21:15:36 -03:00
|
|
|
return (int)constrain(rate, -2500, 2500);
|
2011-04-25 02:12:59 -03:00
|
|
|
}
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-07-10 21:47:08 -03:00
|
|
|
get_stabilize_pitch(long target_angle)
|
2011-01-25 01:53:36 -04:00
|
|
|
{
|
2011-07-10 21:47:08 -03:00
|
|
|
long error;
|
|
|
|
long rate;
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
error = wrap_180(target_angle - dcm.pitch_sensor);
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// limit the error we're feeding to the PID
|
2011-04-10 17:31:33 -03:00
|
|
|
error = constrain(error, -2500, 2500);
|
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
// desired Rate:
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-08-13 06:37:01 -03:00
|
|
|
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
2011-07-10 21:47:08 -03:00
|
|
|
// Rate P:
|
|
|
|
error = rate - (long)(degrees(omega.y) * 100.0);
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
2011-08-13 06:37:01 -03:00
|
|
|
#endif
|
2011-07-10 21:47:08 -03:00
|
|
|
|
|
|
|
// output control:
|
|
|
|
return (int)constrain(rate, -2500, 2500);
|
2011-01-25 01:53:36 -04:00
|
|
|
}
|
|
|
|
|
2011-09-04 21:15:36 -03:00
|
|
|
|
|
|
|
#define YAW_ERROR_MAX 2000
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-09-04 21:15:36 -03:00
|
|
|
get_stabilize_yaw(long target_angle)
|
2011-01-25 01:53:36 -04:00
|
|
|
{
|
2011-07-10 21:47:08 -03:00
|
|
|
long error;
|
|
|
|
long rate;
|
|
|
|
|
2011-09-04 21:15:36 -03:00
|
|
|
yaw_error = wrap_180(target_angle - dcm.yaw_sensor);
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-01-25 01:53:36 -04:00
|
|
|
// limit the error we're feeding to the PID
|
2011-09-04 21:15:36 -03:00
|
|
|
yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
|
2011-01-25 01:53:36 -04:00
|
|
|
|
2011-08-13 06:37:01 -03:00
|
|
|
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
|
|
|
if( ! g.heli_ext_gyro_enabled ) {
|
|
|
|
// Rate P:
|
|
|
|
error = rate - (long)(degrees(omega.z) * 100.0);
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
2011-08-13 06:37:01 -03:00
|
|
|
}
|
|
|
|
#else
|
2011-07-10 21:47:08 -03:00
|
|
|
// Rate P:
|
2011-09-04 21:15:36 -03:00
|
|
|
error = rate - (long)(degrees(omega.z) * 100.0);
|
2011-09-19 18:02:42 -03:00
|
|
|
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
2011-08-13 06:37:01 -03:00
|
|
|
#endif
|
2011-07-10 21:47:08 -03:00
|
|
|
|
|
|
|
// output control:
|
|
|
|
return (int)constrain(rate, -2500, 2500);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-11-01 13:25:23 -03:00
|
|
|
#define ALT_ERROR_MAX 300
|
2011-09-04 21:15:36 -03:00
|
|
|
static int
|
2011-10-27 16:31:46 -03:00
|
|
|
get_nav_throttle(long z_error)
|
2011-09-04 21:15:36 -03:00
|
|
|
{
|
|
|
|
// limit error to prevent I term run up
|
|
|
|
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
2011-10-02 15:36:23 -03:00
|
|
|
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
|
|
|
|
|
|
|
|
rate_error = rate_error - altitude_rate;
|
2011-09-04 21:15:36 -03:00
|
|
|
|
2011-10-02 15:36:23 -03:00
|
|
|
// limit the rate
|
2011-11-01 13:25:23 -03:00
|
|
|
rate_error = constrain(rate_error, -80, 140);
|
2011-09-25 18:16:35 -03:00
|
|
|
return (int)g.pi_throttle.get_pi(rate_error, .1);
|
2011-09-04 21:15:36 -03:00
|
|
|
}
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-10-27 16:31:46 -03:00
|
|
|
get_rate_roll(long target_rate)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-10-27 16:31:46 -03:00
|
|
|
long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
|
|
|
|
return g.pi_acro_roll.get_pi(error, G_Dt);
|
|
|
|
}
|
2011-01-25 01:53:36 -04:00
|
|
|
|
2011-10-27 16:31:46 -03:00
|
|
|
static int
|
|
|
|
get_rate_pitch(long target_rate)
|
|
|
|
{
|
|
|
|
long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
|
|
|
|
return g.pi_acro_pitch.get_pi(error, G_Dt);
|
2011-01-25 01:53:36 -04:00
|
|
|
}
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-07-10 21:47:08 -03:00
|
|
|
get_rate_yaw(long target_rate)
|
|
|
|
{
|
|
|
|
long error;
|
|
|
|
error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
|
2011-09-19 18:02:42 -03:00
|
|
|
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
2011-07-10 21:47:08 -03:00
|
|
|
|
|
|
|
// output control:
|
|
|
|
return (int)constrain(target_rate, -2500, 2500);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
|
|
|
// Keeps outdated data out of our calculations
|
2011-09-04 22:30:31 -03:00
|
|
|
static void reset_hold_I(void)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-09-04 21:15:36 -03:00
|
|
|
g.pi_loiter_lat.reset_I();
|
2011-10-27 16:31:46 -03:00
|
|
|
g.pi_loiter_lon.reset_I();
|
2011-09-04 21:15:36 -03:00
|
|
|
g.pi_crosstrack.reset_I();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-09-04 22:30:31 -03:00
|
|
|
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
|
|
|
// Keeps outdated data out of our calculations
|
2011-09-14 17:58:18 -03:00
|
|
|
static void reset_nav(void)
|
2011-09-04 22:30:31 -03:00
|
|
|
{
|
2011-09-14 17:58:18 -03:00
|
|
|
nav_throttle = 0;
|
|
|
|
invalid_throttle = true;
|
|
|
|
|
2011-09-04 22:30:31 -03:00
|
|
|
g.pi_nav_lat.reset_I();
|
|
|
|
g.pi_nav_lon.reset_I();
|
2011-09-14 17:58:18 -03:00
|
|
|
|
|
|
|
long_error = 0;
|
|
|
|
lat_error = 0;
|
2011-09-04 22:30:31 -03:00
|
|
|
}
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-02-19 22:03:01 -04:00
|
|
|
/*************************************************************
|
|
|
|
throttle control
|
|
|
|
****************************************************************/
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static long
|
2011-07-16 19:12:52 -03:00
|
|
|
get_nav_yaw_offset(int yaw_input, int reset)
|
2011-02-19 22:03:01 -04:00
|
|
|
{
|
2011-07-10 21:47:08 -03:00
|
|
|
long _yaw;
|
2011-02-21 00:30:56 -04:00
|
|
|
|
2011-07-16 19:12:52 -03:00
|
|
|
if(reset == 0){
|
2011-07-10 21:47:08 -03:00
|
|
|
// we are on the ground
|
|
|
|
return dcm.yaw_sensor;
|
2011-02-21 00:30:56 -04:00
|
|
|
|
2011-02-24 01:56:59 -04:00
|
|
|
}else{
|
2011-07-10 21:47:08 -03:00
|
|
|
// re-define nav_yaw if we have stick input
|
|
|
|
if(yaw_input != 0){
|
|
|
|
// set nav_yaw + or - the current location
|
2011-09-14 17:58:18 -03:00
|
|
|
_yaw = (long)yaw_input + dcm.yaw_sensor;
|
2011-07-10 21:47:08 -03:00
|
|
|
// we need to wrap our value so we can be 0 to 360 (*100)
|
|
|
|
return wrap_360(_yaw);
|
2011-07-30 17:42:54 -03:00
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
}else{
|
2011-07-30 17:42:54 -03:00
|
|
|
// no stick input, lets not change nav_yaw
|
2011-07-10 21:47:08 -03:00
|
|
|
return nav_yaw;
|
|
|
|
}
|
2011-02-19 22:03:01 -04:00
|
|
|
}
|
|
|
|
}
|
2011-09-04 21:15:36 -03:00
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int alt_hold_velocity()
|
2011-07-10 21:47:08 -03:00
|
|
|
{
|
2011-09-30 03:27:23 -03:00
|
|
|
#if ACCEL_ALT_HOLD == 1
|
2011-10-27 16:31:46 -03:00
|
|
|
Vector3f accel_filt;
|
|
|
|
float error;
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-10-27 16:31:46 -03:00
|
|
|
// subtract filtered Accel
|
|
|
|
error = abs(next_WP.alt - current_loc.alt) - 25;
|
|
|
|
error = min(error, 50.0);
|
|
|
|
error = max(error, 0.0);
|
|
|
|
error = 1 - (error/ 50.0);
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-10-27 16:31:46 -03:00
|
|
|
accel_filt = imu.get_accel_filtered();
|
|
|
|
accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
|
|
|
|
int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-10-27 16:31:46 -03:00
|
|
|
//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
|
|
|
|
return constrain(output, -70, 70);
|
2011-10-02 15:36:23 -03:00
|
|
|
|
|
|
|
// fast rise
|
|
|
|
//s: -17.6241, g:0.0000, e:1.0000, o:0
|
|
|
|
//s: -18.4990, g:0.0000, e:1.0000, o:0
|
|
|
|
//s: -19.3193, g:0.0000, e:1.0000, o:0
|
|
|
|
//s: -13.1310, g:47.8700, e:1.0000, o:-158
|
|
|
|
|
2011-09-30 03:27:23 -03:00
|
|
|
#else
|
|
|
|
return 0;
|
|
|
|
#endif
|
2011-09-04 21:15:36 -03:00
|
|
|
}
|
2011-07-10 21:47:08 -03:00
|
|
|
|
2011-10-27 16:31:46 -03:00
|
|
|
static int get_angle_boost(int value)
|
2011-02-19 22:03:01 -04:00
|
|
|
{
|
|
|
|
float temp = cos_pitch_x * cos_roll_x;
|
2011-09-23 19:34:18 -03:00
|
|
|
temp = 1.0 - constrain(temp, .5, 1.0);
|
2011-10-27 16:31:46 -03:00
|
|
|
return (int)(temp * value);
|
2011-02-19 22:03:01 -04:00
|
|
|
}
|
|
|
|
|