/*
* From Roll-Pitch Gyro Drift Compensation, Rev 3. William Premerlani, 2012.
*/
static void rollPitch_drift_GPS(float Rbe[3][3], float accels_e_int[3],
float delT_between_updates, float *errRollPitch_b)
{
float errRollPitch_e[3];
float dGPSdt_e[3];
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
dGPSdt_e[0] = (gpsVelocity.North - drft->GPSV_old[0]) / delT_between_updates;
dGPSdt_e[1] = (gpsVelocity.East - drft->GPSV_old[1]) / delT_between_updates;
dGPSdt_e[2] = -GRAVITY + (gpsVelocity.Down - drft->GPSV_old[2]) / delT_between_updates;
drft->GPSV_old[0] = gpsVelocity.North;
drft->GPSV_old[1] = gpsVelocity.East;
drft->GPSV_old[2] = gpsVelocity.Down;
float normdGPSdt_e = VectorMagnitude(dGPSdt_e);
//Take cross product of integrated accelerometer measurements with integrated earth frame accelerations. We should be using normalized dGPSdt, but we perform that calculation in the following line(s).
CrossProduct((const float *)accels_e_int, (const float *)dGPSdt_e, errRollPitch_e);
//Scale cross product
errRollPitch_e[0] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[1] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[2] /= (normdGPSdt_e * delT_between_updates);
//Rotate earth drift error back into body frame;
rot_mult(Rbe, errRollPitch_e, errRollPitch_b, FALSE);
}
/*
* Although yaw correction is done in horizontal plane, it is
* computed in 3 dimensions, just in case we change our minds later.
*/
void yaw_drift_MagGPS(float Rbe[3][3], bool gpsNewData_flag, bool magNewData_flag, float *errYaw_b)
{
#if defined (PIOS_INCLUDE_GPS) || defined (PIOS_INCLUDE_MAGNETOMETER)
// Form the horizontal direction over ground based on rmat
float horizDirOverGndRmat[3];
//Define forward vector in earth frame, Rbe' * [1;0;0], and eliminate vertical component from vector
horizDirOverGndRmat[0] = Rbe[0][0];
horizDirOverGndRmat[1] = Rbe[0][1];
horizDirOverGndRmat[2] = 0;
#if defined (PIOS_INCLUDE_GPS)
if (gpsNewData_flag) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
if (fabs(gpsVelocity.North) > GPS_SPEED_MIN
|| fabs(gpsVelocity.East) > GPS_SPEED_MIN) {
float errorGPSYaw_e;
float errorGPSYaw_b[3];
float normGPS =
sqrtf(gpsVelocity.North * gpsVelocity.North +
gpsVelocity.East * gpsVelocity.East);
float horizDirOverGndGPS[3] = { gpsVelocity.North / normGPS, gpsVelocity.East / normGPS, 0 }; //Normalized vector
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorGPSYaw_e = horizDirOverGndRmat X horizDirOverGndGPS
errorGPSYaw_e =
horizDirOverGndRmat[0] * horizDirOverGndGPS[1] -
horizDirOverGndGPS[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorGPSYaw_b = Rbe * errorGPSYaw_e;
errorGPSYaw_b[0] = Rbe[0][2] * errorGPSYaw_e;
errorGPSYaw_b[1] = Rbe[1][2] * errorGPSYaw_e;
errorGPSYaw_b[2] = Rbe[2][2] * errorGPSYaw_e;
//Sum error with existing error
errYaw_b[0] += errorGPSYaw_b[0] * GPS_YAW_KP;
errYaw_b[1] += errorGPSYaw_b[1] * GPS_YAW_KP;
errYaw_b[2] += errorGPSYaw_b[2] * GPS_YAW_KP;
}
}
#endif
// TODO: Create a flag to indicate whether mag data is present (or check for updates of MagnetometerData)
#if defined (PIOS_INCLUDE_MAGNETOMETER)
if (magNewData_flag) {
MagnetometerData magnetometerData;
MagnetometerGet(&magnetometerData);
float errorMagYaw_e;
float errorMagYaw_b[3];
float horizDirOverGndMag[3];
float mags_b[3] = { magnetometerData.x, magnetometerData.y, magnetometerData.z };
// Rotate magnetometer to earth frame and eliminate vertical component
rot_mult(Rbe, mags_b, horizDirOverGndMag, true);
horizDirOverGndMag[2] = 0;
//Compute norm
float normMag = VectorMagnitude(horizDirOverGndMag);
//Normalize mag_vector. Recall that horizDirOverGndMag[2]=0
horizDirOverGndMag[0] /= normMag;
horizDirOverGndMag[1] /= normMag;
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorMagYaw_e = horizDirOverGndRmat X horizDirOverGndMag
errorMagYaw_e = horizDirOverGndRmat[0] * horizDirOverGndMag[1] -
horizDirOverGndMag[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorMagYaw_b = Rbe * errorMagYaw_e;
errorMagYaw_b[0] = Rbe[0][2] * errorMagYaw_e;
errorMagYaw_b[1] = Rbe[1][2] * errorMagYaw_e;
errorMagYaw_b[2] = Rbe[2][2] * errorMagYaw_e;
errYaw_b[0] += errorMagYaw_b[0] * MAG_YAW_KP;
errYaw_b[1] += errorMagYaw_b[1] * MAG_YAW_KP;
errYaw_b[2] += errorMagYaw_b[2] * MAG_YAW_KP;
}
#endif
#endif
}
/**
* This method performs a simple simulation of a car
*
* It takes in the ActuatorDesired command to rotate the aircraft and performs
* a simple kinetic model where the throttle increases the energy and drag decreases
* it. Changing altitude moves energy from kinetic to potential.
*
* 1. Update attitude based on ActuatorDesired
* 2. Update position based on velocity
*/
static void simulateModelCar()
{
static double pos[3] = {0,0,0};
static double vel[3] = {0,0,0};
static double ned_accel[3] = {0,0,0};
static float q[4] = {1,0,0,0};
static float rpy[3] = {0,0,0}; // Low pass filtered actuator
static float baro_offset = 0.0f;
float Rbe[3][3];
const float ACTUATOR_ALPHA = 0.8;
const float MAX_THRUST = 9.81 * 0.5;
const float K_FRICTION = 0.2;
const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0;
static uint32_t last_time;
float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
if(dT < 1e-3)
dT = 2e-3;
last_time = PIOS_DELAY_GetRaw();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ActuatorDesiredData actuatorDesired;
ActuatorDesiredGet(&actuatorDesired);
float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
if (thrust < 0)
thrust = 0;
if (thrust != thrust)
thrust = 0;
// float control_scaling = thrust * thrustToDegs;
// // In rad/s
// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
/**** 1. Update attitude ****/
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
// Need to get roll angle for easy cross coupling
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
rpy[0] = 0; // cannot roll
rpy[1] = 0; // cannot pitch
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss();
GyrosSet(&gyrosData);
// Predict the attitude forward in time
float qdot[4];
qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
if(overideAttitude){
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
}
/**** 2. Update position based on velocity ****/
// Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed
// we get forward airspeed
Quaternion2R(q,Rbe);
double groundspeed[3] = {vel[0], vel[1], vel[2] };
double forwardSpeed = Rbe[0][0] * groundspeed[0] + Rbe[0][1] * groundspeed[1] + Rbe[0][2] * groundspeed[2];
double sidewaysSpeed = Rbe[1][0] * groundspeed[0] + Rbe[1][1] * groundspeed[1] + Rbe[1][2] * groundspeed[2];
/* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */
/* TODO: This should become more accurate. Use the force equations to calculate lift from the */
/* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */
double forces[3]; // X, Y, Z
forces[0] = thrust - forwardSpeed * K_FRICTION; // Friction is applied in all directions in NED
forces[1] = 0 - sidewaysSpeed * K_FRICTION * 100; // No side slip
forces[2] = 0;
// Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
ned_accel[2] = 0;
// Apply acceleration based on velocity
ned_accel[0] -= K_FRICTION * (vel[0]);
ned_accel[1] -= K_FRICTION * (vel[1]);
// Predict the velocity forward in time
vel[0] = vel[0] + ned_accel[0] * dT;
vel[1] = vel[1] + ned_accel[1] * dT;
vel[2] = vel[2] + ned_accel[2] * dT;
// Predict the position forward in time
pos[0] = pos[0] + vel[0] * dT;
pos[1] = pos[1] + vel[1] * dT;
pos[2] = pos[2] + vel[2] * dT;
// Simulate hitting ground
if(pos[2] > 0) {
pos[2] = 0;
vel[2] = 0;
ned_accel[2] = 0;
}
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
ned_accel[2] -= GRAVITY;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
if(baro_offset == 0) {
// Hacky initialization
baro_offset = 50;// * rand_gauss();
} else {
// Very small drift process
baro_offset += rand_gauss() / 100;
}
// Update baro periodically
static uint32_t last_baro_time = 0;
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
last_baro_time = PIOS_DELAY_GetRaw();
}
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
static float gps_vel_drift[3] = {0,0,0};
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
// Update GPS periodically
static uint32_t last_gps_time = 0;
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
// Use double precision here as simulating what GPS produces
double T[3];
T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
T[2] = -1.0;
static float gps_drift[3] = {0,0,0};
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
AttitudeSimulatedData attitudeSimulated;
AttitudeSimulatedGet(&attitudeSimulated);
attitudeSimulated.q1 = q[0];
attitudeSimulated.q2 = q[1];
attitudeSimulated.q3 = q[2];
attitudeSimulated.q4 = q[3];
Quaternion2RPY(q,&attitudeSimulated.Roll);
attitudeSimulated.Position[0] = pos[0];
attitudeSimulated.Position[1] = pos[1];
attitudeSimulated.Position[2] = pos[2];
attitudeSimulated.Velocity[0] = vel[0];
attitudeSimulated.Velocity[1] = vel[1];
attitudeSimulated.Velocity[2] = vel[2];
AttitudeSimulatedSet(&attitudeSimulated);
}
static void simulateModelQuadcopter()
{
static double pos[3] = {0,0,0};
static double vel[3] = {0,0,0};
static double ned_accel[3] = {0,0,0};
static float q[4] = {1,0,0,0};
static float rpy[3] = {0,0,0}; // Low pass filtered actuator
static float baro_offset = 0.0f;
static float temperature = 20;
float Rbe[3][3];
const float ACTUATOR_ALPHA = 0.8;
const float MAX_THRUST = GRAVITY * 2;
const float K_FRICTION = 1;
const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0;
static uint32_t last_time;
float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
if(dT < 1e-3)
dT = 2e-3;
last_time = PIOS_DELAY_GetRaw();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ActuatorDesiredData actuatorDesired;
ActuatorDesiredGet(&actuatorDesired);
float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
if (thrust < 0)
thrust = 0;
if (thrust != thrust)
thrust = 0;
// float control_scaling = thrust * thrustToDegs;
// // In rad/s
// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
temperature = 20;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11; // - powf(temperature - 20,3) * 0.05;;
gyrosData.y = rpy[1] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
gyrosData.z = rpy[2] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
gyrosData.temperature = temperature;
GyrosSet(&gyrosData);
// Predict the attitude forward in time
float qdot[4];
qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
if(overideAttitude){
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
}
static float wind[3] = {0,0,0};
wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
Quaternion2R(q,Rbe);
// Make thrust negative as down is positive
ned_accel[0] = -thrust * Rbe[2][0];
ned_accel[1] = -thrust * Rbe[2][1];
// Gravity causes acceleration of 9.81 in the down direction
ned_accel[2] = -thrust * Rbe[2][2] + GRAVITY;
// Apply acceleration based on velocity
ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
// Predict the velocity forward in time
vel[0] = vel[0] + ned_accel[0] * dT;
vel[1] = vel[1] + ned_accel[1] * dT;
vel[2] = vel[2] + ned_accel[2] * dT;
// Predict the position forward in time
pos[0] = pos[0] + vel[0] * dT;
pos[1] = pos[1] + vel[1] * dT;
pos[2] = pos[2] + vel[2] * dT;
// Simulate hitting ground
if(pos[2] > 0) {
pos[2] = 0;
vel[2] = 0;
ned_accel[2] = 0;
}
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
ned_accel[2] -= 9.81;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
if(baro_offset == 0) {
// Hacky initialization
baro_offset = 50;// * rand_gauss();
} else {
// Very small drift process
baro_offset += rand_gauss() / 100;
}
// Update baro periodically
static uint32_t last_baro_time = 0;
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
last_baro_time = PIOS_DELAY_GetRaw();
}
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
static float gps_vel_drift[3] = {0,0,0};
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
// Update GPS periodically
static uint32_t last_gps_time = 0;
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
// Use double precision here as simulating what GPS produces
double T[3];
T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
T[2] = -1.0;
static float gps_drift[3] = {0,0,0};
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
gpsPosition.Status = GPSPOSITION_STATUS_FIX3D;
GPSPositionSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
// Run the offset compensation algorithm from the firmware
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
AttitudeSimulatedData attitudeSimulated;
AttitudeSimulatedGet(&attitudeSimulated);
attitudeSimulated.q1 = q[0];
attitudeSimulated.q2 = q[1];
attitudeSimulated.q3 = q[2];
attitudeSimulated.q4 = q[3];
Quaternion2RPY(q,&attitudeSimulated.Roll);
attitudeSimulated.Position[0] = pos[0];
attitudeSimulated.Position[1] = pos[1];
attitudeSimulated.Position[2] = pos[2];
attitudeSimulated.Velocity[0] = vel[0];
attitudeSimulated.Velocity[1] = vel[1];
attitudeSimulated.Velocity[2] = vel[2];
AttitudeSimulatedSet(&attitudeSimulated);
}
/**
* @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
* @params[in] first_run This is the first run so trigger reinitialization
* @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
* @return 0 for success, -1 for failure
*/
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
MagnetometerData magData;
BaroAltitudeData baroData;
GPSPositionData gpsData;
GPSVelocityData gpsVelData;
GyrosBiasData gyrosBias;
static bool mag_updated = false;
static bool baro_updated;
static bool gps_updated;
static bool gps_vel_updated;
static float baroOffset = 0;
static uint32_t ins_last_time = 0;
static bool inited;
float NED[3] = {0.0f, 0.0f, 0.0f};
float vel[3] = {0.0f, 0.0f, 0.0f};
float zeros[3] = {0.0f, 0.0f, 0.0f};
// Perform the update
uint16_t sensors = 0;
float dT;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) )
{
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
if (inited) {
mag_updated = 0;
baro_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
if (first_run) {
inited = false;
init_stage = 0;
mag_updated = 0;
baro_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
// Get most recent data
GyrosGet(&gyrosData);
AccelsGet(&accelsData);
MagnetometerGet(&magData);
BaroAltitudeGet(&baroData);
GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
// Discard mag if it has NAN (normally from bad calibration)
mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z);
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]);
// Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
if (!inited)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else if (outdoor_mode && gpsData.Satellites < 7)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
// Don't initialize until all sensors are read
if (init_stage == 0 && !outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float NED[3];
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Initialize barometric offset to cirrent GPS NED coordinate
baroOffset = -NED[2] - baroData.Altitude;
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(NED, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage > 0) {
// Run prediction a bit before any corrections
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f,
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f,
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
}
init_stage++;
if(init_stage > 10)
inited = true;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
if (!inited)
return 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f)
dT = 0.01f;
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * F_PI / 180.0f;
gyros[1] += gyrosBias.y * F_PI / 180.0f;
gyros[2] += gyrosBias.z * F_PI / 180.0f;
}
// Advance the state estimate
INSStatePrediction(gyros, &accelsData.x, dT);
// Copy the attitude into the UAVO
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
if(mag_updated)
sensors |= MAG_SENSORS;
if(baro_updated)
sensors |= BARO_SENSOR;
INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode)
{
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
vel[2] = 0;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA )
* ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) {
INSSetPosVelVar(1e2f, 1e2f);
vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS |VERT_SENSORS;
}
if (gps_vel_updated && outdoor_mode) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors)
INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors);
// Copy the position and velocity into the UAVO
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = Nav.Pos[0];
positionActual.East = Nav.Pos[1];
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = Nav.Vel[0];
velocityActual.East = Nav.Vel[1];
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
GyrosBiasSet(&gyrosBias);
}
return 0;
}
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
{
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
return -1;
MagnetometerData magData;
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
timeval = PIOS_DELAY_GetRaw();
return 0;
}
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
magKp = 0.01f;
init = 1;
}
GyrosGet(&gyrosData);
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
// Account for accel magnitude
accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
{
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagnetometerData mag;
Quaternion2R(q, Rbe);
MagnetometerGet(&mag);
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
}
if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
// Transform the GPS position into NED coordinates
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsVelocity.North;
velocityActual.East = gpsVelocity.East;
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
return 0;
}
