/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
bool first_run = true;
uint32_t last_algorithm;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(NULL);
// Wait for all the sensors be to read
vTaskDelay(100);
// Invalidate previous algorithm to trigger a first run
last_algorithm = 0xfffffff;
// Main task loop
while (1) {
int32_t ret_val = -1;
if (last_algorithm != revoSettings.FusionAlgorithm) {
last_algorithm = revoSettings.FusionAlgorithm;
first_run = true;
}
// This function blocks on data queue
switch (revoSettings.FusionAlgorithm ) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR:
ret_val = updateAttitudeINSGPS(first_run, false);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
break;
}
if(ret_val == 0)
first_run = false;
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
}
static void SensorsTask(void *parameters)
{
portTickType lastSysTime;
uint32_t accel_samples = 0;
uint32_t gyro_samples = 0;
int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0};
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
UAVObjEvent ev;
settingsUpdatedCb(&ev);
const struct pios_board_info * bdinfo = &pios_board_info_blob;
switch(bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#else
mag_test = 0;
#endif
if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelay(10);
}
}
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
}
for (int i = 0; i < 3; i++) {
accel_accum[i] = 0;
gyro_accum[i] = 0;
}
accel_samples = 0;
gyro_samples = 0;
AccelsData accelsData;
GyrosData gyrosData;
switch(bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
#if defined(PIOS_INCLUDE_BMA180)
{
struct pios_bma180_data accel;
int32_t read_good;
int32_t count;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstarts
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while(read_good == 0) {
count++;
accel_accum[1] += accel.x;
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
}
#endif
#if defined(PIOS_INCLUDE_L3GD20)
{
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
error = true;
continue;
}
gyro_samples = 1;
gyro_accum[1] += gyro.gyro_x;
gyro_accum[0] += gyro.gyro_y;
gyro_accum[2] -= gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyrosData.temperature = gyro.temperature;
}
#endif
break;
case 0x02: // MPU6000 board
case 0x03: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
{
struct pios_mpu6000_data mpu6000_data;
xQueueHandle queue = PIOS_MPU6000_GetQueue();
while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
{
gyro_accum[0] += mpu6000_data.gyro_x;
gyro_accum[1] += mpu6000_data.gyro_y;
gyro_accum[2] += mpu6000_data.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_z;
gyro_samples ++;
accel_samples ++;
}
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
continue;
}
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
// Scale the accels
float accels[3] = {(float) accel_accum[0] / accel_samples,
(float) accel_accum[1] / accel_samples,
(float) accel_accum[2] / accel_samples};
float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
if (rotate) {
rot_mult(R, accels_out, accels);
accelsData.x = accels[0];
accelsData.y = accels[1];
accelsData.z = accels[2];
} else {
accelsData.x = accels_out[0];
accelsData.y = accels_out[1];
accelsData.z = accels_out[2];
}
AccelsSet(&accelsData);
// Scale the gyros
float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
gyrosData.y = gyros[1];
gyrosData.z = gyros[2];
} else {
gyrosData.x = gyros_out[0];
gyrosData.y = gyros_out[1];
gyrosData.z = gyros_out[2];
}
if (bias_correct_gyro) {
// Apply bias correction to the gyros from the state estimator
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
GyrosSet(&gyrosData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = {-(float) values[1] * mag_scale[0] - mag_bias[0],
-(float) values[0] * mag_scale[1] - mag_bias[1],
-(float) values[2] * mag_scale[2] - mag_bias[2]};
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
// Correct for mag bias and update if the rate is non zero
if(cal.MagBiasNullingRate > 0)
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
}
}
/**
* Main task. It does not return.
*/
static void batteryTask(void * parameters)
{
const float dT = SAMPLE_PERIOD_MS / 1000.0f;
settingsUpdatedCb(NULL);
// Main task loop
portTickType lastSysTime;
lastSysTime = xTaskGetTickCount();
while (true) {
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLE_PERIOD_MS));
FlightBatteryStateData flightBatteryData;
FlightBatterySettingsData batterySettings;
float energyRemaining;
if (battery_settings_updated) {
battery_settings_updated = false;
FlightBatterySettingsGet(&batterySettings);
voltageADCPin = batterySettings.VoltagePin;
if (voltageADCPin == FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)
voltageADCPin = -1;
currentADCPin = batterySettings.CurrentPin;
if (currentADCPin == FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)
currentADCPin = -1;
}
//calculate the battery parameters
if (voltageADCPin >= 0) {
flightBatteryData.Voltage = ((float) PIOS_ADC_GetChannelVolt(voltageADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_VOLTAGE] * 1000.0f +
batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_VOLTAGE]; //in Volts
} else {
flightBatteryData.Voltage = 0; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
if (currentADCPin >= 0) {
flightBatteryData.Current = ((float) PIOS_ADC_GetChannelVolt(currentADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_CURRENT] * 1000.0f +
batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_CURRENT]; //in Amps
if (flightBatteryData.Current > flightBatteryData.PeakCurrent)
flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
} else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm
flightBatteryData.Current = -1; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
}
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); //in mAh
//Apply a 2 second rise time low-pass filter to average the current
float alpha = 1.0f - dT / (dT + 2.0f);
flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; //in Amps
energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh
if (flightBatteryData.AvgCurrent > 0)
flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; //in Sec
else
flightBatteryData.EstimatedFlightTime = 9999;
//generate alarms where needed...
if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) {
//FIXME: There's no guarantee that a floating ADC will give 0. So this
// check might fail, even when there's nothing attached.
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);
} else {
// FIXME: should make the timer alarms user configurable
if (flightBatteryData.EstimatedFlightTime < 30)
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.EstimatedFlightTime < 120)
AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);
// FIXME: should make the battery voltage detection dependent on battery type.
/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING])
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
}
FlightBatteryStateSet(&flightBatteryData);
}
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
enum complimentary_filter_status complimentary_filter_status;
complimentary_filter_status = CF_POWERON;
uint32_t arming_count = 0;
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (complimentary_filter_status == CF_POWERON) {
complimentary_filter_status = (xTaskGetTickCount() > 1000) ?
CF_INITIALIZING : CF_POWERON;
} else if(complimentary_filter_status == CF_INITIALIZING &&
(xTaskGetTickCount() < 7000) &&
(xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 0.1f + 0.1f * (xTaskGetTickCount() < 4000);
accelKi = 0.1;
yawBiasRate = 0.1;
accel_filter_enabled = false;
} else if (zero_during_arming &&
(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
// Use a rapidly decrease accelKp to force the attitude to snap back
// to level and then converge more smoothly
if (arming_count < 20)
accelKp = 1.0f;
else if (accelKp > 0.1f)
accelKp -= 0.01f;
arming_count++;
accelKi = 0.1f;
yawBiasRate = 0.1f;
accel_filter_enabled = false;
// Indicate arming so that after arming it reloads
// the normal settings
if (complimentary_filter_status != CF_ARMING) {
accumulate_gyro_zero();
complimentary_filter_status = CF_ARMING;
accumulating_gyro = true;
}
} else if (complimentary_filter_status == CF_ARMING ||
complimentary_filter_status == CF_INITIALIZING) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
if(accel_alpha > 0.0f)
accel_filter_enabled = true;
// If arming that means we were accumulating gyro
// samples. Compute new bias.
if (complimentary_filter_status == CF_ARMING) {
accumulate_gyro_compute();
accumulating_gyro = false;
arming_count = 0;
}
// Indicate normal mode to prevent rerunning this code
complimentary_filter_status = CF_NORMAL;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AccelsData accels;
GyrosData gyros;
int32_t retval = 0;
retval = updateSensorsCC3D(&accels, &gyros);
// During power on set to angle from accel
if (complimentary_filter_status == CF_POWERON) {
float RPY[3];
float theta = atan2f(accels.x, -accels.z);
RPY[1] = theta * RAD2DEG;
RPY[0] = atan2f(-accels.y, -accels.z / cosf(theta)) * RAD2DEG;
RPY[2] = 0;
RPY2Quaternion(RPY, q);
}
// Only update attitude when sensor data is good
if (retval != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Do not update attitude data in simulation mode
if (!AttitudeActualReadOnly())
updateAttitude(&accels, &gyros);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
