[paparazzi-commits] [5303] Update test_passthrough (from Tobi)

[Allen Ibara]

Revision: 5303
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5303
Author:   aibara
Date:     2010-08-10 22:48:21 +0000 (Tue, 10 Aug 2010)
Log Message:
-----------
Update test_passthrough (from Tobi)

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c

Modified: paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c   2010-08-10 
22:26:19 UTC (rev 5302)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c   2010-08-10 
22:48:21 UTC (rev 5303)
@@ -28,26 +28,44 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include "crc.h"
 #include "fms_debug.h"
 #include "fms_spi_link.h"
 #include "fms_autopilot_msg.h"
 
-
-static union AutopilotMessagePT my_buffers[2];
-static struct AutopilotMessagePTUp*   msg_in  = &my_buffers[0].up;
-static struct AutopilotMessagePTDown* msg_out = &my_buffers[1].down;
+static struct AutopilotMessageCRCFrame msg_in;
+static struct AutopilotMessageCRCFrame msg_out;
 static void send_message(void);
+static void print_up_msg(struct AutopilotMessageCRCFrame * msg);
+static void print_down_msg(struct AutopilotMessageCRCFrame * msg);
 
 int main(int argc, char *argv[]) {
 
+  uint32_t us_delay; 
+
   if (spi_link_init()) {
     TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
     return -1;
   }
+  
+  if(argc > 1) { 
+    us_delay = atoi(argv[1]);
+  }
+  else { 
+    us_delay = 1953; 
+  }
+
+  printf("Delay: %dus\n", us_delay); 
+
+  crc__init(0x31); 
+  printf("CRC initialized\n");
+
+  printf("AutopilotMessage size: %d\n",          sizeof(union 
AutopilotMessage));
+  printf("AutopilotMessageCRCFrame size: %d\n",  sizeof(struct 
AutopilotMessageCRCFrame));
+  
   while (1) {
     send_message();
-    usleep(1953);
-    //usleep(50000);
+    usleep(us_delay);
   }
 
   return 0;
@@ -57,16 +75,71 @@
 
 static void send_message() {
        static uint32_t foo = 0;
+  static uint32_t crc_errors = 0; 
+  crc_t crc_in; 
 
-       spi_link_send(msg_out, sizeof(union AutopilotMessagePT), msg_in);
+       spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), 
&msg_in);
+  crc_in      = crc__calc_block_crc8(&msg_in.payload.msg_up, sizeof(union 
AutopilotMessage));
+  
+  if(msg_in.crc != crc_in) {
+    crc_errors++;
+  }
+  
        if (!(foo % 100)) {
-               printf("0x%08x -> gx%+02f gy%+02f gz%+02f ax%+02f ay%+02f 
az%+02f rs%02x\n",
+    printf("CRC errors: %d \n", crc_errors);
+//    print_up_msg(&msg_in);
+//    print_down_msg(&msg_out);
+               printf("0x%08x -> gx%+02f gy%+02f gz%+02f ax%+02f ay%+02f 
az%+02f rs%02x | CRC errors: %d \n",
                        foo,
-                       DegOfRad(RATE_FLOAT_OF_BFP(msg_in->gyro.x)), 
DegOfRad(RATE_FLOAT_OF_BFP(msg_in->gyro.y)), 
DegOfRad(RATE_FLOAT_OF_BFP(msg_in->gyro.z)),
-                       ACCEL_FLOAT_OF_BFP(msg_in->accel.x), 
ACCEL_FLOAT_OF_BFP(msg_in->accel.y), ACCEL_FLOAT_OF_BFP(msg_in->accel.z),
-                       msg_in->rc_status);
+                       
DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.p)), 
+      DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.q)), 
+      DegOfRad(RATE_FLOAT_OF_BFP(msg_in.payload.msg_up.gyro.r)),
+                       ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.x), 
+      ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.y), 
+      ACCEL_FLOAT_OF_BFP(msg_in.payload.msg_up.accel.z),
+                       msg_in.payload.msg_up.rc_status, 
+      crc_errors);
        }
        foo++;
 }
 
 
+static void print_up_msg(struct AutopilotMessageCRCFrame * msg) { 
+  printf("UP: %04X %04X %04X %04X %04x %04X %04X %04X %04X \n", 
+          msg->payload.msg_up.gyro.p, 
+          msg->payload.msg_up.gyro.q, 
+          msg->payload.msg_up.gyro.r, 
+          msg->payload.msg_up.accel.x, 
+          msg->payload.msg_up.accel.y, 
+          msg->payload.msg_up.accel.z, 
+          msg->payload.msg_up.mag.x, 
+          msg->payload.msg_up.mag.y, 
+          msg->payload.msg_up.mag.z);
+  printf("    %04X %04X %04X %04X %04X %04X %04X %04X %04X %02X [%d %d %d %d] 
CRC: %d\n", 
+          msg->payload.msg_up.rc_pitch, 
+          msg->payload.msg_up.rc_roll, 
+          msg->payload.msg_up.rc_yaw, 
+          msg->payload.msg_up.rc_thrust, 
+          msg->payload.msg_up.rc_mode, 
+          msg->payload.msg_up.rc_kill, 
+          msg->payload.msg_up.rc_gear, 
+          msg->payload.msg_up.rc_aux3, 
+          msg->payload.msg_up.rc_aux4, 
+          msg->payload.msg_up.rc_status, 
+          msg->payload.msg_up.valid.rc, 
+          msg->payload.msg_up.valid.pressure, 
+          msg->payload.msg_up.valid.vane, 
+          msg->payload.msg_up.valid.imu, 
+          msg->crc);
+}
+static void print_down_msg(struct AutopilotMessageCRCFrame * msg) { 
+  printf("%04X %04X %04X %04X %04X %04X CRC: %d\n", 
msg->payload.msg_down.pwm_outputs_usecs[0], 
+                                                    
msg->payload.msg_down.pwm_outputs_usecs[1], 
+                                                    
msg->payload.msg_down.pwm_outputs_usecs[2], 
+                                                    
msg->payload.msg_down.pwm_outputs_usecs[3], 
+                                                    
msg->payload.msg_down.pwm_outputs_usecs[4], 
+                                                    
msg->payload.msg_down.pwm_outputs_usecs[5], 
+                                                    msg->crc);
+}
+
+