/** * 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); } } }