*** empty log message ***

sw/airborne/fly_by_wire/3dmg.c

@@ -0,0 +1,140 @@
+
+
+#include "std.h"
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <avr/signal.h>
+#include "3dmg.h"
+#include "uart.h"
+
+volatile bool_t _3dmg_data_ready;
+int16_t   _3dmg_roll, _3dmg_pitch, _3dmg_yaw;
+int16_t   _3dmg_roll_dot, _3dmg_pitch_dot, _3dmg_yaw_dot;
+uint16_t  _3dmg_timer_tick;
+uint8_t   type, state;
+uint16_t  checksum_read, checksum_comp;
+
+void _3dmg_set_continuous_mode ( void ) {
+#define REQ_CONT_LEN 5
+  uint8_t msg[REQ_CONT_LEN] = { 0x10, 0x00, 0x31, 0x00, 0x41};
+  uint8_t i;
+  for (i=0; i<REQ_CONT_LEN; i++)
+    uart_transmit(msg[i]);
+}
+
+void _3dmg_capture_neutral ( void ) {
+#define CAP_GYR_LEN 3
+  uint8_t msg[REQ_CONT_LEN] = { 0x06, 0x00, 0x06};
+  uint8_t i;
+  for (i=0; i<CAP_GYR_LEN; i++)
+    uart_transmit(msg[i]);
+}
+
+
+#define READ_MSB(my_short, my_char) {		 \
+    {						 \
+     my_short = (((int16_t)my_char)<<8)&0xFF00;  \
+     state++;					 \
+    }						 \
+  }
+  
+#define READ_LSB(my_short, my_char) {		\
+    {						\
+    my_short += (((int16_t)my_char)&0x00FF);	\
+    checksum_comp += (uint16_t)my_short;	\
+    state++;					\
+    }						\
+  }
+
+static inline void on_3dmg_receive(uint8_t c) {
+  int16_t foo = 0;
+  switch(state) {
+  case 0:
+    _3dmg_data_ready = FALSE ;
+    if(c==0x31) {
+      type = c;
+      checksum_comp = (int16_t)type;
+      state++ ;
+    }
+    break;
+  case 1:
+    READ_MSB(_3dmg_roll, c);
+    break ;
+  case 2:
+    READ_LSB(_3dmg_roll, c);
+    break ;
+  case 3:
+    READ_MSB(_3dmg_pitch, c);
+    break ;
+  case 4:
+    READ_LSB(_3dmg_pitch, c);
+    break ;
+  case 5:
+    READ_MSB(_3dmg_yaw, c);
+    break ;
+  case 6:
+    READ_LSB(_3dmg_yaw, c);
+    break ;
+  case 7:
+    READ_MSB(foo, c);
+     /* accel_x_msb */
+    break ;
+  case 8:
+    READ_LSB(foo, c);
+    /* accel_x_lsb */
+    break ;
+  case 9:
+    READ_MSB(foo, c);
+    /* accel_y_msb */
+    break ;
+  case 10:
+    READ_LSB(foo, c);
+    /* accel_y_lsb */
+    break ;
+  case 11:
+    READ_MSB(foo, c);
+    /* accel_z_msb */
+    break ;
+  case 12:
+    READ_LSB(foo, c);
+    /* accel_z_lsb */
+    break ;  
+  case 13:
+    READ_MSB(_3dmg_roll_dot, c);
+    break ;
+  case 14:
+    READ_LSB(_3dmg_roll_dot, c);
+    break ;
+  case 15:
+    READ_MSB(_3dmg_pitch_dot, c);
+    break ;
+  case 16:
+    READ_LSB(_3dmg_pitch_dot, c);
+    break ;
+  case 17:
+    READ_MSB(_3dmg_yaw_dot, c);
+    break ;
+  case 18:
+    READ_LSB(_3dmg_yaw_dot, c);
+    break ;  
+  case 19:
+    READ_MSB(_3dmg_timer_tick, c);
+    break ;
+  case 20:
+    READ_LSB(_3dmg_timer_tick, c);
+    break ;
+  case 21:
+    checksum_read = (((uint16_t)c)&0x00FF)<<8; 
+    state++;
+    break ;
+  case 22:
+    checksum_read += ((uint16_t)c)&0x00FF;
+    state = 0 ;
+    //   if (checksum_read == checksum_comp)
+      _3dmg_data_ready = TRUE ;
+    break ;
+  }
+}
+
+
+ReceiveUart(on_3dmg_receive);

sw/airborne/fly_by_wire/3dmg.h

@@ -0,0 +1,13 @@
+#ifndef _3DMG_G
+#define _3DMG_G
+
+#include <inttypes.h>
+#include "std.h"
+
+void _3dmg_capture_gyro_bias ( void );
+void _3dmg_set_continuous_mode ( void );
+extern volatile bool_t _3dmg_data_ready;
+extern int16_t         _3dmg_roll, _3dmg_pitch, _3dmg_yaw;
+extern int16_t         _3dmg_roll_dot, _3dmg_pitch_dot, _3dmg_yaw_dot;
+
+#endif /* _3DMG_G */

sw/airborne/fly_by_wire/Makefile

@@ -55,7 +55,9 @@ $(TARGET).srcs	=							\
 	spi.c								\
 	uart.c                                                          \
 	adc_fbw.c                                                       \
-
+	3dmg.c								\
+	imu.c								\
+	control.c							\
 
 include ../../../conf/Makefile.local
 include ../../../conf/Makefile.avr