Merge branch 'next' into camera_stabilization

flight/CopterControl/System/inc/pios_config.h

@@ -99,10 +99,13 @@
 #define PIOS_SYSTEM_STACK_SIZE          460
 #define PIOS_STABILIZATION_STACK_SIZE   524
 #define PIOS_TELEM_STACK_SIZE           500
-#define PIOS_EVENTDISPATCHER_STACK_SIZE 96
+#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
 //#define PIOS_QUATERNION_STABILIZATION
 
+// This can't be too high to stop eventdispatcher thread overflowing
+#define PIOS_EVENTDISAPTCHER_QUEUE      10
+
 #endif /* PIOS_CONFIG_H */
 /**
  * @}

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);
 
@@ -166,14 +166,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);
 
@@ -201,14 +206,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];
@@ -216,9 +231,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;
@@ -274,6 +292,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)
@@ -311,6 +331,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

flight/UAVObjects/eventdispatcher.c

@@ -27,7 +27,11 @@
 #include "openpilot.h"
 
 // Private constants
+#if defined(PIOS_EVENTDISAPTCHER_QUEUE)
+#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE
+#else
 #define MAX_QUEUE_SIZE 20
+#endif
 
 #if defined(PIOS_EVENTDISPATCHER_STACK_SIZE)
 #define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE

ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp

@@ -291,16 +291,12 @@ void ModelViewGadgetWidget::updateAttitude()
 {
     AttitudeActual::DataFields data = attActual->getData(); // get attitude data
     GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model
-    // create quaternions rotations for each axis
-    QQuaternion qX= QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),data.Pitch);
-    QQuaternion qY= QQuaternion::fromAxisAndAngle(QVector3D(0,1,0),data.Roll);
-    QQuaternion qZ= QQuaternion::fromAxisAndAngle(QVector3D(0,0,1),data.Yaw);
-    QQuaternion quat= qX * qY * qZ; // create the quaternion of all the rotations
-    // pass values to simpler variables
-    double x= quat.vector().x();
-    double y= quat.vector().y();
-    double z= quat.vector().z();
-    double w= quat.scalar();
+    double x= data.q3;
+    double y= data.q2;
+    double z= data.q4;
+    double w= data.q1;
+    if (w == 0.0)
+        w = 1.0;
     // create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix
     QMatrix4x4 m1;
     m1.setRow(0, QVector4D(w,z,-y,x));