updated to new can callback code, main_overo now logs to a file and shuts down in a controlled manner on receiving control-c

conf/airframes/Poine/beth.xml

@@ -156,7 +156,13 @@ main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 #main_stm32.CFLAGS += -DUSE_I2C2
 #main_stm32.srcs += $(SRC_BETH)/bench_sensors_i2c.c
 
-main_stm32.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ
+main_stm32.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DUSE_CAN_EXT_ID
+main_stm32.CFLAGS += \
+	-DCAN_PRESCALER=11 \
+	-DCAN_SJW_TQ=CAN_SJW_1tq \
+	-DCAN_BS1_TQ=CAN_BS1_3tq \
+	-DCAN_BS2_TQ=CAN_BS2_5tq \
+	-DCAN_ERR_RESUME=DISABLE
 main_stm32.srcs += can.c $(SRC_ARCH)/can_hw.c
 main_stm32.srcs += $(SRC_BETH)/bench_sensors_can.c
 

sw/airborne/beth/bench_sensors.h

@@ -12,9 +12,7 @@
 #include "can_hw.h"
 #include <stm32/can.h>
 
-extern volatile uint8_t CAN_RX_FLAG;
+extern uint16_t halfw1,halfw2,halfw3,halfw4;
-extern CanRxMsg can_rx_msg;
-extern uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
 #endif
 
 extern void bench_sensors_init(void);
@@ -45,31 +43,4 @@ extern struct BenchSensors bench_sensors;
   }
 #endif
 
-
-#ifdef USE_CAN1
-#define BenchSensorsEvent( _handler) {		\
-    if (CAN_RX_FLAG == 1) {	\
-	tempid = (uint16_t)(can_rx_msg.ExtId>>7); \
-	if (tempid == 2) { \
-		bench_sensors.current = 2;\
-		halfw2 = can_rx_msg.Data[3]; \
-		halfw2 = (halfw2<<8) + can_rx_msg.Data[2]; \
-		halfw1 = can_rx_msg.Data[1]; \
-		halfw1 = (halfw1<<8) + can_rx_msg.Data[0]; \
-		bench_sensors.angle_2 = halfw1;		\
-		bench_sensors.angle_3 = halfw2;		\
-	} else { \
-		bench_sensors.current = 1;\
-		halfw4 = can_rx_msg.Data[3]; \
-		halfw4 = (halfw4<<8) + can_rx_msg.Data[2]; \
-		halfw3 = can_rx_msg.Data[1]; \
-		halfw3 = (halfw3<<8) + can_rx_msg.Data[0]; \
-		bench_sensors.angle_1 = halfw4;		\
-	} \
-      CAN_RX_FLAG = 0;					\
-      _handler();							\
-    }									\
-  }
-#endif
-
 #endif /* BENCH_SENSORS_H  */

sw/airborne/beth/bench_sensors_can.c

@@ -1,25 +1,26 @@
 #include "bench_sensors.h"
 #include "can.h"
+#include "led.h"
 
-uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
+uint16_t halfw1,halfw2,halfw3,halfw4;
 
 struct BenchSensors bench_sensors;
 
-static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len);
+static void can_rx_callback(uint32_t id, uint8_t *buf, int len);
+
 
 void bench_sensors_init(void) {
-  can_init(can_rx_callback_t);
+  can_init(can_rx_callback);
 }
 
 void read_bench_sensors(void) {
   
 }
 
-
+static void can_rx_callback(uint32_t id, uint8_t *buf, int len) {
-static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len) {
+  //LED_TOGGLE(7);
-  
+  bench_sensors.current = id>>7;
-  if (id == 2) {
+  if (bench_sensors.current == 2) {
-    bench_sensors.current = 2;
     halfw2 = buf[3];
     halfw2 = (halfw2<<8) + buf[2];
     halfw1 = buf[1];
@@ -28,11 +29,10 @@ static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len) {
     bench_sensors.angle_3 = halfw2;
   } 
   else {
-    bench_sensors.current = 1;
     halfw4 = buf[3];
     halfw4 = (halfw4<<8) + buf[2];
-    halfw3 = buf[1];
+    //halfw3 = buf[1];
-    halfw3 = (halfw3<<8) + buf[0];
+    //halfw3 = (halfw3<<8) + buf[0];
     bench_sensors.angle_1 = halfw4;
   }
 

sw/airborne/beth/main_overo.c

@@ -26,6 +26,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <signal.h>
 
 #include "fms_network.h"
 #include "fms_periodic.h"
@@ -53,6 +54,8 @@ static struct AutopilotMessageBethUp   msg_in;
 static struct AutopilotMessageBethDown msg_out;
 static void send_message(void);
 static void PID(void);
+void ex_program(int sig);
+FILE *outfile;
 
 struct BoozImu booz_imu;
 struct BoozImuFloat booz_imu_float;
@@ -64,12 +67,12 @@ static uint32_t foo = 0;
 
 static void main_periodic(int my_sig_num) {
 
-  RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
 
   PID();
   send_message();
 
-  RunOnceEvery(10, {DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
+  RunOnceEvery(50, {DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
     &msg_in.bench_sensor.z,&foo);});
 
   booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
@@ -81,6 +84,9 @@ static void main_periodic(int my_sig_num) {
   
   BoozImuScaleGyro();
 
+  fprintf(outfile,"%f %d IMU_ACCEL_RAW %d %d %d\n",foo/500.,42,booz_imu.accel_unscaled.x,booz_imu.accel_unscaled.y,booz_imu.accel_unscaled.z);
+  fprintf(outfile,"%f %d IMU_GYRO_RAW %d %d %d\n",foo/500.,42,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r);
+
   RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
 			     //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
 				&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
@@ -90,23 +96,23 @@ static void main_periodic(int my_sig_num) {
 			     //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
 				&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
 
-  RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+  RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
 			     //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
 				&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
     
 
-  RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+/*  RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
 			     //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
-				&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});
+				&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
 
-  RunOnceEvery(10, {UdpTransportPeriodic();});
+  RunOnceEvery(33, {UdpTransportPeriodic();});
 
 }
 
 static int8_t pitchval = 0;
 static float kp, ki, kd;
 int8_t presp,dresp;
-static uint16_t tilt_sp=2600;
+static uint16_t tilt_sp=2770;
 static float piderror,piderrorold;
 
 static void PID(){
@@ -125,6 +131,13 @@ static void PID(){
   }
 }
 
+void ex_program(int sig) {
+  fprintf(outfile,"%d\n",foo);
+  printf("Closing down\n");
+  fclose(outfile);
+  exit(EXIT_SUCCESS);
+}
+
 int main(int argc, char *argv[]) {
 
   if (argc>1){
@@ -143,6 +156,10 @@ int main(int argc, char *argv[]) {
   }
   ki=0.0;
 
+  outfile = fopen("output.data","w+");
+
+  (void) signal(SIGINT, ex_program);
+
   RATES_ASSIGN(booz_imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);
   VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
   VECT3_ASSIGN(booz_imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL,   IMU_MAG_Z_NEUTRAL);

sw/airborne/beth/main_stm32.c

@@ -43,7 +43,6 @@ static inline void on_mag_event(void);
 
 static inline void main_on_overo_msg_received(void);
 static inline void main_on_overo_link_lost(void);
-static inline void main_on_bench_sensors( void );
 
 static int16_t my_cnt;
 
@@ -66,7 +65,7 @@ static inline void main_init( void ) {
   booz_imu_init();
   overo_link_init();
   bench_sensors_init();
-  LED_ON(7);
+  //LED_ON(7);
 }
 
 
@@ -114,7 +113,6 @@ static inline void main_event( void ) {
   BoozImuEvent(on_gyro_accel_event, on_mag_event);
   OveroLinkEvent(main_on_overo_msg_received);
 
-  BenchSensorsEvent(main_on_bench_sensors);
 }
 
 static inline void main_on_overo_msg_received(void) {
@@ -207,7 +205,3 @@ static inline void on_mag_event(void) {
   }
 }
 
-
-static inline void main_on_bench_sensors( void ) {
- 
-}