/**
* Perform an update of the @ref MagBias based on
* Magnetometer Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagnetometerData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = {0, 0, 0};
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = {mag->x, mag->y, mag->z};
float norm_diff = sqrtf(powf(B2[0] - B1[0],2) + powf(B2[1] - B1[1],2) + powf(B2[2] - B1[2],2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = cal.MagBiasNullingRate;
float R[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, R);
// Rotate the mag into the NED frame
B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
float cy = cosf(attitude.Yaw * M_PI / 180.0f);
float sy = sinf(attitude.Yaw * M_PI / 180.0f);
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) {
magBias.x += delta[0];
magBias.y += delta[1];
magBias.z += delta[2];
MagBiasSet(&magBias);
}
#endif
}
static void gpsTask(void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
GPSPositionData GpsData;
#ifdef ENABLE_GPS_BINARY_GTOP
GTOP_BIN_init();
#else
uint8_t rx_count = 0;
bool start_flag = false;
bool found_cr = false;
int32_t gpsRxOverflow = 0;
#endif
#ifdef FULL_COLD_RESTART
// tell the GPS to do a FULL COLD restart
PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n");
timeOfLastCommandMs = timeNowMs;
while (timeNowMs - timeOfLastCommandMs < 300) // delay for 300ms to let the GPS sort itself out
{
vTaskDelay(xDelay); // Block task until next update
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
}
#endif
#ifdef DISABLE_GPS_TRESHOLD
PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n");
#endif
#ifdef ENABLE_GPS_BINARY_GTOP
// switch to GTOP binary mode
PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
#endif
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
// switch to single sentence mode
PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n");
#endif
#ifdef ENABLE_GPS_NMEA
// switch to NMEA mode
PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n");
#endif
numUpdates = 0;
numChecksumErrors = 0;
numParsingErrors = 0;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
// Loop forever
while (1)
{
#ifdef ENABLE_GPS_BINARY_GTOP
// GTOP BINARY GPS mode
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
if (res >= 0)
{
numUpdates++;
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
#else
// NMEA or SINGLE-SENTENCE GPS mode
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
char c = PIOS_COM_ReceiveBuffer(gpsPort);
// detect start while acquiring stream
if (!start_flag && (c == '$'))
{
start_flag = true;
found_cr = false;
rx_count = 0;
}
else
if (!start_flag)
continue;
if (rx_count >= sizeof(gps_rx_buffer))
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
}
else
{
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r') )
found_cr = true;
else
if (found_cr && (c != '\n') )
found_cr = false; // false end flag
else
if (found_cr && (c == '\n') )
{
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count-2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1]))
{ // Invalid checksum. May indicate dropped characters on Rx.
PIOS_DEBUG_PinHigh(2);
++numChecksumErrors;
PIOS_DEBUG_PinLow(2);
}
else
{ // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1])) {
PIOS_DEBUG_PinHigh(2);
++numParsingErrors;
PIOS_DEBUG_PinLow(2);
}
else
++numUpdates;
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
}
#endif
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
{ // we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GPSPositionGet(&GpsData);
GpsData.Status = GPSPOSITION_STATUS_NOGPS;
GPSPositionSet(&GpsData);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)
{ // resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command
timeOfLastCommandMs = timeNowMs;
#ifdef ENABLE_GPS_BINARY_GTOP
GTOP_BIN_init();
// switch to binary mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n");
#endif
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
// switch to single sentence mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n");
#endif
#ifdef ENABLE_GPS_NMEA
// switch to NMEA mode
PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
#endif
#ifdef DISABLE_GPS_TRESHOLD
PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n");
#endif
}
}
else
{ // we appear to be receiving GPS sentences OK, we've had an update
HomeLocationData home;
HomeLocationGet(&home);
GPSPositionGet(&GpsData);
if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
setHomeLocation(&GpsData);
//criteria for GPS-OK taken from this post...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
else
if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
// Block task until next update
vTaskDelay(xDelay);
}
}
/**
* 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);
}
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
/* Normaly we get/set UAVObjects but this one only needs to be set.
We will never expect to get this from another module*/
AttitudeActualData attitude_actual;
AHRSSettingsData ahrs_settings;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config( adc_oversampling );
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable( 0 );
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy(( char * )mag_data.id, "ZZZ" );
PIOS_HMC5843_ReadID( mag_data.id );
#endif
/* SPI link to master */
// PIOS_SPI_Init();
AhrsInitComms();
AHRSCalibrationConnectCallback( calibration_callback );
GPSPositionConnectCallback( gps_callback );
ahrs_state = AHRS_IDLE;
while( !AhrsLinkReady() ) {
AhrsPoll();
while( ahrs_state != AHRS_DATA_READY ) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
ahrs_state = AHRS_IDLE;
if(( total_conversion_blocks % 50 ) == 0 )
PIOS_LED_Toggle( LED1 );
}
AHRSSettingsGet(&ahrs_settings);
/* Use simple averaging filter for now */
for( int i = 0; i < adc_oversampling; i++ )
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) {
// compute a data point and initialize INS
downsample_data();
converge_insgps();
}
#ifdef DUMP_RAW
int previous_conversion;
while( 1 ) {
AhrsPoll();
int result;
uint8_t framing[16] = {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15
};
while( ahrs_state != AHRS_DATA_READY ) ;
ahrs_state = AHRS_PROCESSING;
if( total_conversion_blocks != previous_conversion + 1 )
PIOS_LED_On( LED1 ); // not keeping up
else
PIOS_LED_Off( LED1 );
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header
result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & valid_data_buffer[0],
ADC_OVERSAMPLE *
ADC_CONTINUOUS_CHANNELS *
sizeof( valid_data_buffer[0] ) );
if( result == 0 )
PIOS_LED_Off( LED1 );
else {
PIOS_LED_On( LED1 );
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while( 1 ) {
AhrsPoll();
AHRSCalibrationData calibration;
AHRSCalibrationGet( &calibration );
BaroAltitudeData baro_altitude;
BaroAltitudeGet( &baro_altitude );
GPSPositionData gps_position;
GPSPositionGet( &gps_position );
AHRSSettingsGet(&ahrs_settings);
// Alive signal
if(( total_conversion_blocks % 100 ) == 0 )
PIOS_LED_Toggle( LED1 );
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if( PIOS_HMC5843_NewDataAvailable() ) {
PIOS_HMC5843_ReadMag( mag_data.raw.axis );
mag_data.updated = 1;
}
attitude_raw.magnetometers[0] = mag_data.raw.axis[0];
attitude_raw.magnetometers[2] = mag_data.raw.axis[1];
attitude_raw.magnetometers[2] = mag_data.raw.axis[2];
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while( ahrs_state != AHRS_DATA_READY ) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/***************** SEND BACK SOME RAW DATA ************************/
// Hacky - grab one sample from buffer to populate this. Need to send back
// all raw data if it's happening
accel_data.raw.x = valid_data_buffer[0];
accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7];
if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) {
/******************** INS ALGORITHM **************************/
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] );
mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] );
mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] );
INSStatePrediction( gyro, accel,
1 / ( float )EKF_RATE );
INSCovariancePrediction( 1 / ( float )EKF_RATE );
if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_position.Groundspeed *
cos( gps_position.Heading * M_PI / 180 );
vel[1] =
gps_position.Groundspeed *
sin( gps_position.Heading * M_PI / 180 );
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar( 0.004 );
HomeLocationData home;
HomeLocationGet( &home );
float ned[3];
double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )};
// convert from cm back to meters
double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )};
LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned );
if( gps_updated ) { //FIXME: Is this correct?
//TOOD: add check for altitude updates
FullCorrection( mag, ned,
vel,
baro_altitude.Altitude );
gps_updated = false;
} else {
GpsBaroCorrection( ned,
vel,
baro_altitude.Altitude );
}
gps_updated = false;
mag_data.updated = 0;
} else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D
&& mag_data.updated == 1 ) {
MagCorrection( mag ); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar( 0.1 );
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection( vel,
baro_altitude.Altitude );
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
}
attitude_actual.q1 = Nav.q[0];
attitude_actual.q2 = Nav.q[1];
attitude_actual.q3 = Nav.q[2];
attitude_actual.q4 = Nav.q[3];
} else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2(( mag_data.raw.axis[0] ),
( -1 * mag_data.raw.axis[1] ) ) * 180 /
M_PI;
rpy[1] =
atan2( accel_data.filtered.x,
accel_data.filtered.z ) * 180 / M_PI;
rpy[0] =
atan2( accel_data.filtered.y,
accel_data.filtered.z ) * 180 / M_PI;
RPY2Quaternion( rpy, q );
attitude_actual.q1 = q[0];
attitude_actual.q2 = q[1];
attitude_actual.q3 = q[2];
attitude_actual.q4 = q[3];
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n",
( int16_t )( accel_data.filtered.x * 1000 ),
( int16_t )( accel_data.filtered.y * 1000 ),
( int16_t )( accel_data.filtered.z * 1000 ) );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n",
( int16_t )( gyro_data.filtered.x * 1000 ),
( int16_t )( gyro_data.filtered.y * 1000 ),
( int16_t )( gyro_data.filtered.z * 1000 ) );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2] );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
( int16_t )( Nav.q[0] * 1000 ),
( int16_t )( Nav.q[1] * 1000 ),
( int16_t )( Nav.q[2] * 1000 ),
( int16_t )( Nav.q[3] * 1000 ) );
#endif
#ifdef DUMP_EKF
uint8_t framing[16] = {
15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0
};
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header
result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & mag_data,
sizeof( mag_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & gps_data,
sizeof( gps_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & accel_data,
sizeof( accel_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & gyro_data,
sizeof( gyro_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q,
sizeof( float ) * NUMX * NUMX );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K,
sizeof( float ) * NUMX * NUMV );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X,
sizeof( float ) * NUMX * NUMX );
if( result == 0 )
PIOS_LED_Off( LED1 );
else {
PIOS_LED_On( LED1 );
}
#endif
AttitudeActualSet( &attitude_actual );
/*FIXME: This is dangerous. There is no locking for UAVObjects
so it could stomp all over the airspeed/climb rate etc.
This used to be done in the OP module which was bad.
Having ~4ms latency for the round trip makes it worse here.
*/
PositionActualData pos;
PositionActualGet( &pos );
for( int ct = 0; ct < 3; ct++ ) {
pos.NED[ct] = Nav.Pos[ct];
pos.Vel[ct] = Nav.Vel[ct];
}
PositionActualSet( &pos );
static bool was_calibration = false;
AhrsStatusData status;
AhrsStatusGet( &status );
if( was_calibration != status.CalibrationSet ) {
was_calibration = status.CalibrationSet;
if( status.CalibrationSet ) {
calibrate_sensors();
AhrsStatusGet( &status );
status.CalibrationSet = true;
}
}
status.CPULoad = (( float )running_counts /
( float )( idle_counts + running_counts ) ) * 100;
status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 );
status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 );
status.DroppedUpdates = ekf_too_slow;
AhrsStatusSet( &status );
}
return 0;
}
static void gpsTask(void *parameters)
{
portTickType xDelay = MS2TICKS(GPS_COM_TIMEOUT_MS);
uint32_t timeNowMs = TICKS2MS(xTaskGetTickCount());
GPSPositionData gpsposition;
uint8_t gpsProtocol;
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
gps_airspeed_initialize();
#endif
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
#if !defined(PIOS_GPS_MINIMAL)
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
{
uint8_t gpsAutoConfigure;
ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);
if (gpsAutoConfigure == MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {
// Wait for power to stabilize before talking to external devices
vTaskDelay(MS2TICKS(1000));
// Runs through a number of possible GPS baud rates to
// configure the ublox baud rate. This uses a NMEA string
// so could work for either UBX or NMEA actually. This is
// somewhat redundant with updateSettings below, but that
// is only called on startup and is not an issue.
ModuleSettingsGPSSpeedOptions baud_rate;
ModuleSettingsGPSSpeedGet(&baud_rate);
ubx_cfg_set_baudrate(gpsPort, baud_rate);
vTaskDelay(MS2TICKS(1000));
ubx_cfg_send_configuration(gpsPort, gps_rx_buffer);
}
}
break;
#endif
}
#endif /* PIOS_GPS_MINIMAL */
GPSPositionGet(&gpsposition);
// Loop forever
while (1)
{
uint8_t c;
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
{
int res;
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
}
if (res == PARSER_COMPLETE) {
timeNowMs = TICKS2MS(xTaskGetTickCount());
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// Check for GPS timeout
timeNowMs = TICKS2MS(xTaskGetTickCount());
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
// we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE)
setHomeLocation(&gpsposition);
#endif
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}
}
