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

flight/modules/Attitude/revolution/attitude.c

@@ -75,10 +75,12 @@
 #include "CoordinateConversions.h"
 
 // Private constants
-#define STACK_SIZE_BYTES    2048
+#define STACK_SIZE_BYTES          2048
-#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3)
+#define TASK_PRIORITY             (tskIDLE_PRIORITY + 3)
-#define FAILSAFE_TIMEOUT_MS 10
+#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)) {
+    if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) &&
-        // For first 7 seconds use accels to get gyro bias
+        (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 (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
-    if (variance_error) {
+        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+    } else if (variance_error) {
         AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
     } else {
         AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

flight/modules/GPS/GPS.c

@@ -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;
 }

flight/modules/System/systemmod.c

@@ -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 ?
+        int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2);
-                        SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
-                        SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
 
         if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
             // If object persistence is updated call the callback