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

[Paul Cox]

Revision: 5203
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5203
Author:   paulcox
Date:     2010-08-03 14:35:47 +0000 (Tue, 03 Aug 2010)
Log Message:
-----------
updated to new can callback code, main_overo now logs to a file and shuts down 
in a controlled manner on receiving control-c

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
    paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-03 13:21:17 UTC 
(rev 5202)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-03 14:35:47 UTC 
(rev 5203)
@@ -156,7 +156,13 @@
 #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
 

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-08-03 13:21:17 UTC 
(rev 5202)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-08-03 14:35:47 UTC 
(rev 5203)
@@ -12,9 +12,7 @@
 #include "can_hw.h"
 #include <stm32/can.h>
 
-extern volatile uint8_t CAN_RX_FLAG;
-extern CanRxMsg can_rx_msg;
-extern uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
+extern uint16_t halfw1,halfw2,halfw3,halfw4;
 #endif
 
 extern void bench_sensors_init(void);
@@ -45,31 +43,4 @@
   }
 #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  */

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-03 
13:21:17 UTC (rev 5202)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-03 
14:35:47 UTC (rev 5203)
@@ -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_t(uint32_t id, uint8_t *buf, int len) {
-  
-  if (id == 2) {
-    bench_sensors.current = 2;
+static void can_rx_callback(uint32_t id, uint8_t *buf, int len) {
+  //LED_TOGGLE(7);
+  bench_sensors.current = id>>7;
+  if (bench_sensors.current == 2) {
     halfw2 = buf[3];
     halfw2 = (halfw2<<8) + buf[2];
     halfw1 = buf[1];
@@ -28,11 +29,10 @@
     bench_sensors.angle_3 = halfw2;
   } 
   else {
-    bench_sensors.current = 1;
     halfw4 = buf[3];
     halfw4 = (halfw4<<8) + buf[2];
-    halfw3 = buf[1];
-    halfw3 = (halfw3<<8) + buf[0];
+    //halfw3 = buf[1];
+    //halfw3 = (halfw3<<8) + buf[0];
     bench_sensors.angle_1 = halfw4;
   }
 

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-03 13:21:17 UTC 
(rev 5202)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-03 14:35:47 UTC 
(rev 5203)
@@ -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 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 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 @@
   
   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 @@
                             //&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 @@
   }
 }
 
+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 @@
   }
   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);

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-03 13:21:17 UTC 
(rev 5202)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-03 14:35:47 UTC 
(rev 5203)
@@ -43,7 +43,6 @@
 
 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 @@
   booz_imu_init();
   overo_link_init();
   bench_sensors_init();
-  LED_ON(7);
+  //LED_ON(7);
 }
 
 
@@ -114,7 +113,6 @@
   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 main_on_bench_sensors( void ) {
- 
-}