rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/OP-1009_reset_gyrobias' into next

Browse Source
This commit is contained in:
Alessio Morale 2013-06-18 20:39:55 +02:00
commit f41f7ade66
3 changed files with 44 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
flight/modules/Attitude/revolution/attitude.c
View File

@ -75,10 +75,12 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define FAILSAFE_TIMEOUT_MS 10
#define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define FAILSAFE_TIMEOUT_MS 10
#define CALIBRATION_DELAY 4000
#define CALIBRATION_DURATION 6000
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
@ -319,6 +321,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
float dT;
static uint8_t init = 0;
static bool magCalibrated = true;
static uint32_t initStartupTime = 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 ||
@ -389,11 +392,28 @@ static int32_t updateAttitudeComplementary(bool first_run)
timeval = PIOS_DELAY_GetRaw();
// wait calibration_delay only at powerup
if (xTaskGetTickCount() < 3000) {
initStartupTime = 0;
} else {
initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY;
}
// Zero gyro bias
// This is really needed after updating calibration settings.
GyrosBiasData zeroGyrosBias;
GyrosBiasGet(&zeroGyrosBias);
zeroGyrosBias.x = 0;
zeroGyrosBias.y = 0;
zeroGyrosBias.z = 0;
GyrosBiasSet(&zeroGyrosBias);
return 0;
}
if ((xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) {
// For first 7 seconds use accels to get gyro bias
if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) &&
(xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) {
// For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup
// Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate.
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
@ -619,8 +639,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
}
}
if (variance_error) {
if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

2
flight/modules/GPS/GPS.c
View File

@ -112,8 +112,6 @@ int32_t GPSStart(void)
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
}
return -1;
}

24
flight/modules/System/systemmod.c
View File

@ -167,7 +167,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
{
/* start the delayed callback scheduler */
CallbackSchedulerStart();
uint8_t cycleCount = 0;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
@ -204,6 +204,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
while (1) {
// Update the system statistics
updateStats();
cycleCount = cycleCount > 0 ? cycleCount - 1 : 7;
// Update the system alarms
updateSystemAlarms();
@ -220,26 +221,33 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
uint8_t armingStatus;
FlightStatusArmedGet(&armingStatus);
if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) ||
(armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
#if defined(PIOS_LED_ALARM)
if (AlarmsHasWarnings()) {
if (AlarmsHasCritical()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else if ((AlarmsHasErrors() && (cycleCount & 0x1)) ||
(!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
}
#endif /* PIOS_LED_ALARM */
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
// If object persistence is updated call the callback