Attitude: Do not start attitude estimation until accel data appears. This

fixes the NaN when AttitudeSettings not available.

flight/Modules/Attitude/attitude.c

@@ -76,7 +76,7 @@ static void AttitudeTask(void *parameters);
 static float gyro_correct_int[3] = {0,0,0};
 static xQueueHandle gyro_queue;
 
-static void updateSensors(AttitudeRawData *);
+static int8_t updateSensors(AttitudeRawData *);
 static void updateAttitude(AttitudeRawData *);
 static void settingsUpdatedCb(UAVObjEvent * objEv);
 
@@ -162,14 +162,19 @@ static void AttitudeTask(void *parameters)
 	// Keep flash CS pin high while talking accel
 	PIOS_FLASH_DISABLE;
 	PIOS_ADXL345_Init();
-
-
+	
+	// Set critical error and wait until the accel is producing data
+	while(PIOS_ADXL345_FifoElements() == 0) {
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
+		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
+	}
+	
 	// Force settings update to make sure rotation loaded
 	settingsUpdatedCb(AttitudeSettingsHandle());
 
 	// Main task loop
 	while (1) {
-
+	
 		FlightStatusData flightStatus;
 		FlightStatusGet(&flightStatus);
 
@@ -197,14 +202,24 @@ static void AttitudeTask(void *parameters)
 
 		AttitudeRawData attitudeRaw;
 		AttitudeRawGet(&attitudeRaw);
-		updateSensors(&attitudeRaw);
-		updateAttitude(&attitudeRaw);
-		AttitudeRawSet(&attitudeRaw);
+		if(updateSensors(&attitudeRaw) != 0)
+			AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
+		else {
+			// Only update attitude when sensor data is good
+			updateAttitude(&attitudeRaw);
+			AttitudeRawSet(&attitudeRaw);
+			AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+		}
 
 	}
 }
 
-static void updateSensors(AttitudeRawData * attitudeRaw)
+/**
+ * Get an update from the sensors
+ * @param[in] attitudeRaw Populate the UAVO instead of saving right here
+ * @return 0 if successfull, -1 if not
+ */
+static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 {
 	struct pios_adxl345_data accel_data;
 	float gyro[4];
@@ -212,9 +227,12 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
 	// Only wait the time for two nominal updates before setting an alarm
 	if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
 		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
-		return;
+		return -1;
 	}
 
+	// No accel data available
+	if(PIOS_ADXL345_FifoElements() == 0)
+		return -1;
 
 	// First sample is temperature
 	attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
@@ -270,6 +288,8 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
 	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
 	// and make it average zero (weakly)
 	gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
+	
+	return 0;
 }
 
 static void updateAttitude(AttitudeRawData * attitudeRaw)
@@ -307,6 +327,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
 		// 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;
+			
 		//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
 
 		// Correct rates based on error, integral component dealt with in updateSensors

flight/PiOS/Common/pios_adxl345.c

@@ -96,6 +96,22 @@ void PIOS_ADXL345_Init()
 	PIOS_ADXL345_SetMeasure(1); 
 }
 
+/**
+ * @brief Return number of entries in the fifo
+ */
+uint8_t  PIOS_ADXL345_FifoElements()
+{
+	uint8_t buf[2] = {0,0};
+	uint8_t rec[2] = {0,0};
+	buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status
+	
+	PIOS_ADXL345_ClaimBus();
+	PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
+	PIOS_ADXL345_ReleaseBus();
+	
+	return rec[1] & 0x3f;
+}
+
 /**
  * @brief Read a single set of values from the x y z channels
  * @returns The number of samples remaining in the fifo

flight/PiOS/inc/pios_adxl345.h

@@ -16,6 +16,7 @@
 #define ADXL_MULTI_BIT     0x40
 
 #define ADXL_X0_ADDR       0x32
+#define ADXL_FIFOSTATUS_ADDR 0x39
 
 #define ADXL_RATE_ADDR     0x2C
 #define ADXL_RATE_100      0x0A
@@ -52,5 +53,6 @@ void PIOS_ADXL345_FifoDepth(uint8_t depth);
 void PIOS_ADXL345_Attach(uint32_t spi_id);
 void PIOS_ADXL345_Init();
 uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data);
+uint8_t  PIOS_ADXL345_FifoElements();
 
 #endif