[paparazzi-commits] [5202] added callback for new can implementation

[antoine drouin]

Revision: 5202
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5202
Author:   poine
Date:     2010-08-03 13:21:17 +0000 (Tue, 03 Aug 2010)
Log Message:
-----------
added callback for new can implementation

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-02 
23:42:57 UTC (rev 5201)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-03 
13:21:17 UTC (rev 5202)
@@ -5,11 +5,35 @@
 
 struct BenchSensors bench_sensors;
 
+static void can_rx_callback_t(uint32_t id, uint8_t *buf, int len);
+
 void bench_sensors_init(void) {
-  can_init();
+  can_init(can_rx_callback_t);
 }
 
 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;
+    halfw2 = buf[3];
+    halfw2 = (halfw2<<8) + buf[2];
+    halfw1 = buf[1];
+    halfw1 = (halfw1<<8) + buf[0];
+    bench_sensors.angle_2 = halfw1;
+    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];
+    bench_sensors.angle_1 = halfw4;
+  }
+
+}