[paparazzi-commits] [5062] Keep track of new radio control messages and create new bitfield in the AutopilotMessageUp to indicate when IMU , RC, pressure, or vane fields contain new measurement ( as opposed to old one)

[Allen Ibara]

Revision: 5062
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5062
Author:   aibara
Date:     2010-07-16 03:41:27 +0000 (Fri, 16 Jul 2010)
Log Message:
-----------
Keep track of new radio control messages and create new bitfield in the 
AutopilotMessageUp to indicate when IMU, RC, pressure, or vane fields contain 
new measurement (as opposed to old one)

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
    paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c
    paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c

Modified: paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-07-16 
02:59:52 UTC (rev 5061)
+++ paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-07-16 
03:41:27 UTC (rev 5062)
@@ -55,6 +55,16 @@
 /*
  * Passthrough, aka biplan
  */
+
+/* used to indicate parts of the message which actually represent a new 
measurement */
+struct PTUpValidFlags
+{
+  unsigned rc:1;
+  unsigned pressure:1;
+  unsigned vane:1;
+  unsigned imu:1;
+};
+
 struct __attribute__ ((packed)) AutopilotMessagePTUp
 {
   struct Int32Rates gyro;
@@ -70,6 +80,7 @@
   int16_t rc_aux3;
   int16_t rc_aux4;
   uint8_t rc_status;
+  struct PTUpValidFlags valid;
 };
 
 struct __attribute__ ((packed)) AutopilotMessagePTDown

Modified: paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c    2010-07-16 
02:59:52 UTC (rev 5061)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c    2010-07-16 
03:41:27 UTC (rev 5062)
@@ -97,11 +97,12 @@
 
 static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
 {
-  // FIXME: Should only callback when there is something new
-  // FIXME: placeholders since the vane and pressure data fields don't exist 
yet
-  if (vane_callback)
+
+  if (msg_up->valid.vane && vane_callback)
+    // FIXME: placeholders since the vane and pressure data fields don't exist 
yet
     vane_callback(0, 0., 0.);
-  if (pressure_callback)
+
+  if (msg_up->valid.pressure && pressure_callback)
     pressure_callback(0, 0, 0);
 
   // Fill radio data
@@ -116,7 +117,7 @@
   radio_control.values[RADIO_CONTROL_AUX4] = msg_up->rc_aux4;
   radio_control.status = msg_up->rc_status;
   
-  if (radio_control_callback)
+  if (msg_up->valid.rc && radio_control_callback)
     radio_control_callback();
 
   // Fill IMU data
@@ -132,7 +133,8 @@
   imu.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
   imu.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
 
-  rdyb_booz_imu_update(&imu);
+  if (msg_up->valid.imu)
+    rdyb_booz_imu_update(&imu);
 }
 
 static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_out)
@@ -144,8 +146,8 @@
 
 void spi_ap_link_periodic()
 {
-  struct AutopilotMessagePTUp msg_in;
-  struct AutopilotMessagePTDown msg_out;
+  static struct AutopilotMessagePTUp msg_in;
+  static struct AutopilotMessagePTDown msg_out;
 
   passthrough_down_fill(&msg_out);
 

Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c       
2010-07-16 02:59:52 UTC (rev 5061)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c       
2010-07-16 03:41:27 UTC (rev 5062)
@@ -41,6 +41,8 @@
 static inline void main_on_overo_msg_received(void);
 static inline void main_on_overo_link_lost(void);
 
+static bool_t new_radio_msg;
+
 int main(void) {
 
        main_init();
@@ -54,6 +56,10 @@
        return 0;
 }
 
+static void on_rc_message(void) {
+  new_radio_msg = TRUE;
+}
+
 static inline void main_init(void) {
 
        hw_init();
@@ -75,10 +81,17 @@
 
        BoozImuEvent(on_gyro_accel_event, on_mag_event);
        OveroLinkEvent(main_on_overo_msg_received);
-       RadioControlEvent(NULL);
+       RadioControlEvent(on_rc_message);
 }
 
 static inline void main_on_overo_msg_received(void) {
+
+       if (new_radio_msg) overo_link.up.msg.valid.rc = 1;
+       else overo_link.up.msg.valid.rc = 0;
+        new_radio_msg = FALSE;
+
+       overo_link.up.msg.valid.imu = 1;
+
        RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro);
 
        VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel);