/*
setup the ADC channels with new input
Note that this uses roll, pitch and yaw only as inputs. The
simulator rollrates are instantaneous, whereas we need to use
average rates to cope with slow update rates.
inputs are in degrees
phi - roll
theta - pitch
psi - true heading
alpha - angle of attack
beta - side slip
phidot - roll rate
thetadot - pitch rate
psidot - yaw rate
v_north - north velocity in local/body frame
v_east - east velocity in local/body frame
v_down - down velocity in local/body frame
A_X_pilot - X accel in body frame
A_Y_pilot - Y accel in body frame
A_Z_pilot - Z accel in body frame
Note: doubles on high prec. stuff are preserved until the last moment
*/
void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth
double rollRate, double pitchRate,double yawRate, // Local to plane
double xAccel, double yAccel, double zAccel, // Local to plane
float airspeed)
{
static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 };
static const float _sensor_signs[6] = { 1, -1, -1, 1, -1, -1 };
const float accel_offset = 2041;
const float gyro_offset = 1658;
const float _gyro_gain_x = ToRad(0.4);
const float _gyro_gain_y = ToRad(0.41);
const float _gyro_gain_z = ToRad(0.41);
const float _accel_scale = 9.80665 / 423.8;
double adc[7];
double p, q, r;
extern float sitl_motor_speed[4];
bool motors_on = false;
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
for (uint8_t i=0; i<4; i++) {
if (sitl_motor_speed[i] > 0.0) {
motors_on = true;
}
}
// minimum noise levels are 2 bits
float accel_noise = _accel_scale*2;
float gyro_noise = _gyro_gain_y*2;
if (motors_on) {
// add extra noise when the motors are on
accel_noise += sitl.accel_noise;
gyro_noise += ToRad(sitl.gyro_noise);
}
xAccel += accel_noise * rand_float();
yAccel += accel_noise * rand_float();
zAccel += accel_noise * rand_float();
p += gyro_noise * rand_float();
q += gyro_noise * rand_float();
r += gyro_noise * rand_float();
p += gyro_drift();
q += gyro_drift();
r += gyro_drift();
/* work out the ADC channel values */
adc[0] = (p / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset;
adc[1] = (q / (_gyro_gain_y * _sensor_signs[1])) + gyro_offset;
adc[2] = (r / (_gyro_gain_z * _sensor_signs[2])) + gyro_offset;
adc[3] = (xAccel / (_accel_scale * _sensor_signs[3])) + accel_offset;
adc[4] = (yAccel / (_accel_scale * _sensor_signs[4])) + accel_offset;
adc[5] = (zAccel / (_accel_scale * _sensor_signs[5])) + accel_offset;
/* tell the UDR2 register emulation what values to give to the driver */
for (uint8_t i=0; i<6; i++) {
UDR2.set(sensor_map[i], adc[i]);
}
// set the airspeed sensor input
UDR2.set(7, airspeed_sensor(airspeed));
/* FIX: rubbish value for temperature for now */
UDR2.set(3, 2000);
runInterrupt(6);
}
/*
* Correct sensor drift, using the DCM approach from W. Premerlani et. al
*/
void Premerlani_GPS(float *accels, float *gyros, float Rbe[3][3], const float delT, bool GPS_Drift_Compensation, GlobalAttitudeVariables *glblAtt, float *omegaCorrP)
{
float errYaw_b[3] = { 0, 0, 0 };
float errRollPitch_b[3] = { 0, 0, 0 };
float normOmegaScalar = VectorMagnitude(gyros);
//Correct roll-pitch drift via GPS and accelerometer
//The math is derived from Roll-Pitch Gyro Drift Compensation, Rev.3, by W. Premerlani
#if defined (PIOS_INCLUDE_GPS)
if (drft->gpsPresent_flag && GPS_Drift_Compensation) {
float accels_e[3];
//Rotate accelerometer readings into Earth frame. Note that we need to take the transpose of Rbe.
rot_mult(Rbe, accels, accels_e, TRUE);
//Integrate accelerometer measurements in Earth frame
drft->accels_e_integrator[0] += accels_e[0] * delT;
drft->accels_e_integrator[1] += accels_e[1] * delT;
drft->accels_e_integrator[2] += accels_e[2] * delT;
drft->delT_between_GPS += delT;
//Check if the GPS has new information.
if (!
(drft->
gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_RPY)) {
//Compute drift correction, errRollPitch_b, from GPS
rollPitch_drift_GPS(Rbe, drft->accels_e_integrator,
drft->delT_between_GPS,
errRollPitch_b);
//Reset integrator
memset(drft->accels_e_integrator, 0,
sizeof(drft->accels_e_integrator));
//Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |=
GPS_CONSUMED_BY_RPY;
drft->delT_between_GPS = 0;
}
}
#endif
if (!GPS_Drift_Compensation) {
#if defined (PIOS_INCLUDE_GPS) && 0 || defined (PIOS_INCLUDE_MAGNETOMETER)
if (!(drft->gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_Y)) {
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, true, drft->magNewData_flag, errYaw_b);
// Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |= GPS_CONSUMED_BY_Y;
} else {
// In addition to calculating the roll-pitch-yaw error, we can calculate yaw drift, errYaw_b, based on GPS and attitude data
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, false, drft->magNewData_flag, errYaw_b);
}
// Reset flag. Not the best place to do it, but it's messy anywhere else
if (drft->magNewData_flag) {
drft->magNewData_flag = false;
}
#endif
//In addition, we can calculate roll-pitch error with only the aid of an accelerometer
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualData airspeedActualData;
AirspeedActualGet(&airspeedActualData);
float airspeed_tas = airspeedActualData.TrueAirspeed;
#else
float airspeed_tas = 0;
#endif
rollPitch_drift_accel(accels, gyros, Rbe, airspeed_tas,
errRollPitch_b);
}
// Calculate gyro drift, based on all errors
gyro_drift(gyros, errYaw_b, errRollPitch_b, normOmegaScalar, delT, omegaCorrP, drft->omegaCorrI);
//Calculate final drift response
gyros[0] += omegaCorrP[0] + drft->omegaCorrI[0];
gyros[1] += omegaCorrP[1] + drft->omegaCorrI[1];
gyros[2] += omegaCorrP[2] + drft->omegaCorrI[2];
//Add 0.0001% of proportional error back into gyroscope bias offset. This keeps DC elements out of the raw gyroscope data.
glblAtt->gyro_correct_int[0] += omegaCorrP[0] / 1000000.0f;
glblAtt->gyro_correct_int[1] += omegaCorrP[1] / 1000000.0f;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
glblAtt->gyro_correct_int[2] += -gyros[2] * glblAtt->yawBiasRate;
}
