Functions for getting accel angles, send them over mavlink

Author: grantmd

Accel.c

@@ -5,6 +5,8 @@
     Read accelerometer data over SPI: the LIS302DL
 */
 
+#include <math.h>
+
 #include "ch.h"
 #include "hal.h"
 
@@ -45,4 +47,16 @@ void AccelRead(void){
     accel_x = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX);
     accel_y = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY);
     accel_z = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ);
+}
+
+float AccelGetRollAngle(void){
+	return atan2( accel_x, sqrt( pow(accel_y, 2) + pow(accel_z, 2) ) );
+}
+
+float AccelGetPitchAngle(void){
+	return atan2( accel_y, sqrt( pow(accel_x, 2) + pow(accel_z, 2) ) );
+}
+
+float AccelGetYawAngle(void){
+	return atan2( sqrt( pow(accel_x, 2) + pow(accel_y, 2) ), accel_z );
 }

Accel.h

@@ -14,4 +14,8 @@ extern int8_t accel_x, accel_y, accel_z;
 void AccelInit(void);
 void AccelRead(void);
 
+float AccelGetRollAngle(void);
+float AccelGetPitchAngle(void);
+float AccelGetYawAngle(void);
+
 #endif /* _ACCEL_H_ */

main.c

@@ -150,7 +150,7 @@ int main(void){
 			 * 10hz task loop
 	 		 */
 			if (frameCounter % 10 == 0){
-				//CommsSendAttitude(ST2MS(currentTime - startTime), float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed);
+				CommsSendAttitude(ST2MS(currentTime - startTime), AccelGetRollAngle(), AccelGetPitchAngle(), AccelGetYawAngle(), 0, 0, 0);
 			}
 
 			/*