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
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-11-07 18:24:32 -04:00
|
|
|
get_stabilize_roll(int32_t target_angle)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error;
|
|
|
|
int32_t rate;
|
2011-01-25 01:53:36 -04:00
|
|
|
|
2011-12-03 21:54:38 -04:00
|
|
|
// angle error
|
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-12-08 08:30:47 -04:00
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
// limit the error we're feeding to the PID
|
|
|
|
error = constrain(error, -4500, 4500);
|
2011-12-11 03:25:52 -04:00
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
// convert to desired Rate:
|
|
|
|
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
|
2011-12-11 03:25:52 -04:00
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
// output control:
|
|
|
|
rate = constrain(rate, -4500, 4500);
|
|
|
|
return (int)rate;
|
|
|
|
#else
|
2011-07-10 21:47:08 -03:00
|
|
|
// limit the error we're feeding to the PID
|
|
|
|
error = constrain(error, -2500, 2500);
|
|
|
|
|
2011-12-03 21:54:38 -04:00
|
|
|
// conver to desired Rate:
|
2011-12-07 01:03:56 -04:00
|
|
|
rate = g.pi_stabilize_roll.get_p(error);
|
2011-12-03 21:54:38 -04:00
|
|
|
|
|
|
|
// experiment to pipe iterm directly into the output
|
2011-12-07 01:03:56 -04:00
|
|
|
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
2011-12-11 03:25:52 -04:00
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
// rate control
|
2011-12-03 21:54:38 -04:00
|
|
|
error = rate - (omega.x * DEGX100);
|
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
|
|
|
|
|
|
|
// output control:
|
2011-12-03 21:54:38 -04:00
|
|
|
rate = constrain(rate, -2500, 2500);
|
|
|
|
return (int)rate + iterm;
|
2011-12-08 08:30:47 -04:00
|
|
|
#endif
|
2011-04-25 02:12:59 -03:00
|
|
|
}
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-11-07 18:24:32 -04:00
|
|
|
get_stabilize_pitch(int32_t target_angle)
|
2011-01-25 01:53:36 -04:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error;
|
|
|
|
int32_t rate;
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-12-03 21:54:38 -04:00
|
|
|
// angle error
|
2011-07-10 21:47:08 -03:00
|
|
|
error = wrap_180(target_angle - dcm.pitch_sensor);
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
2010-12-19 12:40:33 -04:00
|
|
|
// limit the error we're feeding to the PID
|
2011-12-08 08:30:47 -04:00
|
|
|
error = constrain(error, -4500, 4500);
|
2011-12-11 03:25:52 -04:00
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
// convert to desired Rate:
|
|
|
|
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
|
|
|
|
|
|
|
|
// output control:
|
|
|
|
rate = constrain(rate, -4500, 4500);
|
|
|
|
return (int)rate;
|
|
|
|
#else
|
|
|
|
// angle error
|
2011-04-10 17:31:33 -03:00
|
|
|
error = constrain(error, -2500, 2500);
|
|
|
|
|
2011-12-07 01:03:56 -04:00
|
|
|
// convert to desired Rate:
|
|
|
|
rate = g.pi_stabilize_pitch.get_p(error);
|
2011-12-03 21:54:38 -04:00
|
|
|
|
|
|
|
// experiment to pipe iterm directly into the output
|
2011-12-08 09:23:50 -04:00
|
|
|
int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-12-03 21:54:38 -04:00
|
|
|
error = rate - (omega.y * DEGX100);
|
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
|
|
|
|
|
|
|
// output control:
|
2011-12-03 21:54:38 -04:00
|
|
|
rate = constrain(rate, -2500, 2500);
|
|
|
|
return (int)rate + iterm;
|
2011-12-08 08:30:47 -04:00
|
|
|
#endif
|
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-11-07 18:24:32 -04:00
|
|
|
get_stabilize_yaw(int32_t target_angle)
|
2011-01-25 01:53:36 -04:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error;
|
|
|
|
int32_t rate;
|
2011-07-10 21:47:08 -03:00
|
|
|
|
2011-12-07 01:03:56 -04:00
|
|
|
// angle error
|
|
|
|
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-12-07 01:03:56 -04:00
|
|
|
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
|
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
// convert to desired Rate:
|
2011-12-08 09:23:50 -04:00
|
|
|
rate = g.pi_stabilize_yaw.get_p(error);
|
|
|
|
|
|
|
|
// experiment to pipe iterm directly into the output
|
|
|
|
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
|
2011-12-11 03:25:52 -04:00
|
|
|
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
2011-12-08 09:23:50 -04:00
|
|
|
if( !g.heli_ext_gyro_enabled ) {
|
|
|
|
error = rate - (omega.z * DEGX100);
|
2011-12-07 01:03:56 -04:00
|
|
|
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
2011-08-13 06:37:01 -03:00
|
|
|
}
|
2011-11-05 01:31:30 -03:00
|
|
|
// output control:
|
2011-12-07 01:03:56 -04:00
|
|
|
rate = constrain(rate, -4500, 4500);
|
2011-08-13 06:37:01 -03:00
|
|
|
#else
|
2011-12-08 09:23:50 -04:00
|
|
|
error = rate - (omega.z * DEGX100);
|
2011-12-07 01:03:56 -04:00
|
|
|
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
2011-11-07 02:45:07 -04:00
|
|
|
|
2011-07-10 21:47:08 -03:00
|
|
|
// output control:
|
2011-12-07 01:03:56 -04:00
|
|
|
rate = constrain(rate, -2500, 2500);
|
2011-11-05 01:31:30 -03:00
|
|
|
#endif
|
2011-12-08 09:23:50 -04:00
|
|
|
|
|
|
|
return (int)rate + iterm;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-11-01 13:25:23 -03:00
|
|
|
#define ALT_ERROR_MAX 300
|
2011-12-05 00:51:34 -04:00
|
|
|
static int16_t
|
2011-11-07 18:24:32 -04:00
|
|
|
get_nav_throttle(int32_t z_error)
|
2011-09-04 21:15:36 -03:00
|
|
|
{
|
2011-12-05 00:51:34 -04:00
|
|
|
int16_t rate_error;
|
|
|
|
|
2011-12-11 03:25:52 -04:00
|
|
|
float dt = (abs(z_error) < 400) ? .1 : 0.0;
|
|
|
|
//float dt = .1;
|
2011-12-05 00:51:34 -04:00
|
|
|
|
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-12-05 00:51:34 -04:00
|
|
|
|
2011-12-07 01:03:56 -04:00
|
|
|
// convert to desired Rate:
|
|
|
|
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
|
2011-12-05 00:51:34 -04:00
|
|
|
|
2011-12-07 01:03:56 -04:00
|
|
|
// experiment to pipe iterm directly into the output
|
|
|
|
int16_t iterm = g.pi_alt_hold.get_i(z_error, dt);
|
2011-12-05 00:51:34 -04:00
|
|
|
|
|
|
|
// calculate rate error
|
2011-11-05 01:41:51 -03:00
|
|
|
rate_error = rate_error - climb_rate;
|
2011-09-04 21:15:36 -03:00
|
|
|
|
2011-10-02 15:36:23 -03:00
|
|
|
// limit the rate
|
2011-12-05 00:51:34 -04:00
|
|
|
rate_error = constrain((int)g.pi_throttle.get_pi(rate_error, .1), -120, 180);
|
|
|
|
|
|
|
|
// output control:
|
|
|
|
return rate_error + iterm;
|
2011-09-04 21:15:36 -03:00
|
|
|
}
|
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static int
|
2011-11-07 18:24:32 -04:00
|
|
|
get_rate_roll(int32_t target_rate)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error = (target_rate * 3.5) - (degrees(omega.x) * 100.0);
|
2011-10-27 16:31:46 -03:00
|
|
|
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
|
2011-11-07 18:24:32 -04:00
|
|
|
get_rate_pitch(int32_t target_rate)
|
2011-10-27 16:31:46 -03:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error = (target_rate * 3.5) - (degrees(omega.y) * 100.0);
|
2011-10-27 16:31:46 -03:00
|
|
|
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-11-07 18:24:32 -04:00
|
|
|
get_rate_yaw(int32_t target_rate)
|
2011-07-10 21:47:08 -03:00
|
|
|
{
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t error;
|
|
|
|
error = (target_rate * 4.5) - (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();
|
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-11-13 01:39:00 -04:00
|
|
|
nav_throttle = 0;
|
|
|
|
invalid_throttle = true;
|
2011-09-14 17:58:18 -03:00
|
|
|
|
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
|
|
|
|
2011-11-13 01:39:00 -04:00
|
|
|
circle_angle = 0;
|
|
|
|
crosstrack_error = 0;
|
|
|
|
nav_lat = 0;
|
|
|
|
nav_lon = 0;
|
|
|
|
nav_roll = 0;
|
|
|
|
nav_pitch = 0;
|
|
|
|
target_bearing = 0;
|
|
|
|
wp_distance = 0;
|
|
|
|
wp_totalDistance = 0;
|
|
|
|
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-11-07 18:24:32 -04:00
|
|
|
int32_t _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-11-07 18:24:32 -04:00
|
|
|
_yaw = 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-11-02 01:18:47 -03:00
|
|
|
static int get_angle_boost(int value)
|
2011-07-10 21:47:08 -03:00
|
|
|
{
|
2011-11-02 01:18:47 -03:00
|
|
|
float temp = cos_pitch_x * cos_roll_x;
|
|
|
|
temp = 1.0 - constrain(temp, .5, 1.0);
|
|
|
|
return (int)(temp * value);
|
|
|
|
}
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
// Accelerometer Z dampening by Aurelio R. Ramos
|
|
|
|
// ---------------------------------------------
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
#if ACCEL_ALT_HOLD == 1
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
// contains G and any other DC offset
|
|
|
|
static float estimatedAccelOffset = 0;
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
// state
|
|
|
|
static float synVelo = 0;
|
|
|
|
static float synPos = 0;
|
|
|
|
static float synPosFiltered = 0;
|
|
|
|
static float posError = 0;
|
|
|
|
static float prevSensedPos = 0;
|
2011-10-02 15:36:23 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
// tuning for dead reckoning
|
2011-11-07 02:45:07 -04:00
|
|
|
static const float dt_50hz = 0.02;
|
|
|
|
static float synPosP = 10 * dt_50hz;
|
|
|
|
static float synPosI = 15 * dt_50hz;
|
|
|
|
static float synVeloP = 1.5 * dt_50hz;
|
|
|
|
static float maxVeloCorrection = 5 * dt_50hz;
|
2011-11-02 01:18:47 -03:00
|
|
|
static float maxSensedVelo = 1;
|
2011-11-07 02:45:07 -04:00
|
|
|
static float synPosFilter = 0.5;
|
2011-11-02 01:18:47 -03:00
|
|
|
|
|
|
|
#define NUM_G_SAMPLES 200
|
|
|
|
|
|
|
|
// Z damping term.
|
2011-11-07 02:45:07 -04:00
|
|
|
static float fullDampP = 0.100;
|
2011-11-02 01:18:47 -03:00
|
|
|
|
|
|
|
float get_world_Z_accel()
|
|
|
|
{
|
|
|
|
Vector3f accels_rot = dcm.get_dcm_matrix() * imu.get_accel();
|
|
|
|
return accels_rot.z;
|
2011-09-04 21:15:36 -03:00
|
|
|
}
|
2011-07-10 21:47:08 -03:00
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
|
|
|
|
static void init_z_damper()
|
2011-02-19 22:03:01 -04:00
|
|
|
{
|
2011-11-02 01:18:47 -03:00
|
|
|
estimatedAccelOffset = 0;
|
|
|
|
for (int i = 0; i < NUM_G_SAMPLES; i++){
|
|
|
|
estimatedAccelOffset += get_world_Z_accel();
|
|
|
|
}
|
|
|
|
estimatedAccelOffset /= (float)NUM_G_SAMPLES;
|
|
|
|
}
|
|
|
|
|
|
|
|
float dead_reckon_Z(float sensedPos, float sensedAccel)
|
|
|
|
{
|
|
|
|
// the following algorithm synthesizes position and velocity from
|
|
|
|
// a noisy altitude and accelerometer data.
|
|
|
|
|
|
|
|
// synthesize uncorrected velocity by integrating acceleration
|
2011-11-07 02:45:07 -04:00
|
|
|
synVelo += (sensedAccel - estimatedAccelOffset) * dt_50hz;
|
2011-11-02 01:18:47 -03:00
|
|
|
|
|
|
|
// synthesize uncorrected position by integrating uncorrected velocity
|
2011-11-07 02:45:07 -04:00
|
|
|
synPos += synVelo * dt_50hz;
|
2011-11-02 01:18:47 -03:00
|
|
|
|
|
|
|
// filter synPos, the better this filter matches the filtering and dead time
|
|
|
|
// of the sensed position, the less the position estimate will lag.
|
|
|
|
synPosFiltered = synPosFiltered * (1 - synPosFilter) + synPos * synPosFilter;
|
|
|
|
|
|
|
|
// calculate error against sensor position
|
|
|
|
posError = sensedPos - synPosFiltered;
|
|
|
|
|
|
|
|
// correct altitude
|
|
|
|
synPos += synPosP * posError;
|
|
|
|
|
|
|
|
// correct integrated velocity by posError
|
|
|
|
synVelo = synVelo + constrain(posError, -maxVeloCorrection, maxVeloCorrection) * synPosI;
|
|
|
|
|
|
|
|
// correct integrated velocity by the sensed position delta in a small proportion
|
|
|
|
// (i.e., the low frequency of the delta)
|
2011-11-07 02:45:07 -04:00
|
|
|
float sensedVelo = (sensedPos - prevSensedPos) / dt_50hz;
|
2011-11-02 01:18:47 -03:00
|
|
|
synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP;
|
|
|
|
|
|
|
|
prevSensedPos = sensedPos;
|
|
|
|
return synVelo;
|
|
|
|
}
|
|
|
|
|
|
|
|
static int get_z_damping()
|
|
|
|
{
|
|
|
|
float sensedAccel = get_world_Z_accel();
|
|
|
|
float sensedPos = current_loc.alt / 100.0;
|
|
|
|
|
|
|
|
float synVelo = dead_reckon_Z(sensedPos, sensedAccel);
|
|
|
|
return constrain(fullDampP * synVelo * (-1), -300, 300);
|
2011-02-19 22:03:01 -04:00
|
|
|
}
|
|
|
|
|
2011-11-02 01:18:47 -03:00
|
|
|
#else
|
|
|
|
|
|
|
|
static int get_z_damping()
|
|
|
|
{
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2011-12-08 08:30:47 -04:00
|
|
|
#endif
|