static uint32_t get_time(void)
{
portTickType ticks;
ticks = xTaskGetTickCount();
return TICKS2MS(ticks);
}
uint32_t GetSystimeMS(void)
{
return (uint32_t)TICKS2MS(xTaskGetTickCount());
}
PmReturn_t plat_getMsTicks(uint32_t *r_ticks)
{
*r_ticks = TICKS2MS(xTaskGetTickCount());
return PM_RET_OK;
}
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
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
} 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);
}
}
}
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
#ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0;
if(airspeedSensorType != AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
//Fetch calibrated airspeed from sensors
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
{ //Have to catch the fallthrough, or else this loop will monopolize the processor!
airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE;
airspeedData.SensorValue=12345;
//Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
}
#ifdef GPS_AIRSPEED_PRESENT
float v_air_GPS=-1.0f;
//Check if sufficient time has passed. This will depend on whether we have a pitot tube
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT
float delT = TICKS2MS(lastSysTime - lastLoopTime) / 1000.0f;
lastLoopTime=lastSysTime;
if ( (TICKS2MS(lastSysTime - lastGPSTime) > 1000 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani
gps_airspeedGet(&v_air_GPS);
}
//Use the GPS error to correct the airspeed estimate.
if (v_air_GPS > 0) //We have valid GPS estimate...
{
airspeedData.GPSAirspeed=v_air_GPS;
#ifdef BARO_AIRSPEED_PRESENT
if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present...
//Calculate error and error integral
float airspeedErr=v_air_GPS - airspeed_tas_baro;
airspeedErrInt+=airspeedErr * delT;
//Saturate integral component at 5 m/s
airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
{
//...there's no airspeed sensor, so everything comes from GPS. In this
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
airspeedData.CalibratedAirspeed = tas2cas(airspeedData.TrueAirspeed, -positionActual_Down, &air_STP);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin_dummy)
{
//Find out which sensor we're using.
switch (airspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
break;
}
}
#endif
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}
/**
*
* @brief Suspends execution of current thread for a regular interval.
*
* @param[in] previous_ms pointer to system time of last execution,
* must have been initialized with PIOS_Thread_Systime() on first invocation
* @param[in] increment_ms time of regular interval in milliseconds
*
*/
void PIOS_Thread_Sleep_Until(uint32_t *previous_ms, uint32_t increment_ms)
{
portTickType temp = MS2TICKS(*previous_ms);
vTaskDelayUntil(&temp, (portTickType)MS2TICKS(increment_ms));
*previous_ms = TICKS2MS(temp);
}
/**
*
* @brief Returns the current system time.
*
* @returns current system time
*
*/
uint32_t PIOS_Thread_Systime(void)
{
return (uint32_t)TICKS2MS(xTaskGetTickCount());
}
static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
static float accels_filtered[3] = {0,0,0};
static float grot_filtered[3] = {0,0,0};
dT = (thisSysTime == lastSysTime) ? 0.001f : TICKS2MS(portMAX_DELAY & (thisSysTime - lastSysTime)) / 1000.0f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
float * gyros = &gyrosData->x;
float * accels = &accelsData->x;
float grot[3];
float accel_err[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(accels,accels_filtered);
// Rotate gravity to body frame, filter 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]);
// Apply same filtering to the rotated attitude to match delays
apply_accel_filter(grot,grot_filtered);
// Compute the error between the predicted direction of gravity and smoothed acceleration
CrossProduct((const float *) accels_filtered, (const float *) grot_filtered, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]);
// Account for filtered gravity vector magnitude
float grot_mag;
if (accel_filter_enabled)
grot_mag = sqrtf(grot_filtered[0]*grot_filtered[0] + grot_filtered[1]*grot_filtered[1] + grot_filtered[2]*grot_filtered[2]);
else
grot_mag = 1.0f;
if (grot_mag > 1.0e-3f && accel_mag > 1.0e-3f) {
accel_err[0] /= (accel_mag*grot_mag);
accel_err[1] /= (accel_mag*grot_mag);
accel_err[2] /= (accel_mag*grot_mag);
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] -= accel_err[0] * accelKi;
gyro_correct_int[1] -= accel_err[1] * accelKi;
// Correct rates based on error, integral component dealt with in updateSensors
gyros[0] += accel_err[0] * accelKp / dT;
gyros[1] += accel_err[1] * accelKp / dT;
gyros[2] += accel_err[2] * accelKp / dT;
}
{ // scoping variables to save memory
// 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] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[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];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
// Renomalize
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 quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
}
