[paparazzi-commits] [6057] fixing sensors passing

[antoine drouin]

Revision: 6057
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6057
Author:   poine
Date:     2010-10-04 10:04:18 +0000 (Mon, 04 Oct 2010)
Log Message:
-----------
fixing sensors passing

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
    paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
    paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
    paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_overo_link.c

Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c    2010-10-03 
21:57:03 UTC (rev 6056)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c    2010-10-04 
10:04:18 UTC (rev 6057)
@@ -233,10 +233,16 @@
 #endif
     ins_propagate();
   }
+#ifdef USE_VEHICLE_INTERFACE
+  vi_notify_imu_available();
+#endif
 }
 
 static inline void on_baro_abs_event( void ) {
   ins_update_baro();
+#ifdef USE_VEHICLE_INTERFACE
+  vi_notify_baro_abs_available();
+#endif
 }
 
 static inline void on_baro_dif_event( void ) {
@@ -245,10 +251,17 @@
 
 static inline void on_gps_event(void) {
   ins_update_gps();
+#ifdef USE_VEHICLE_INTERFACE
+  if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)
+    vi_notify_gps_available();
+#endif
 }
 
 static inline void on_mag_event(void) {
   ImuScaleMag(imu);
   if (ahrs.status == AHRS_RUNNING)
     ahrs_update_mag();
+#ifdef USE_VEHICLE_INTERFACE
+  vi_notify_mag_available();
+#endif
 }

Modified: paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-10-03 
21:57:03 UTC (rev 6056)
+++ paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-10-04 
10:04:18 UTC (rev 6057)
@@ -118,10 +118,10 @@
 
 
 
-#define IMU_DATA_VALID      0
-#define MAG_DATA_VALID      1
-#define GPS_DATA_VALID      2
-#define BARO_ABS_DATA_VALID 3
+#define VI_IMU_DATA_VALID      0
+#define VI_MAG_DATA_VALID      1
+#define VI_GPS_DATA_VALID      2
+#define VI_BARO_ABS_DATA_VALID 3
 
 struct __attribute__ ((packed)) AutopilotMessageVIUp 
 {
@@ -130,7 +130,7 @@
   struct Int16Vect3 mag;
   struct EcefCoor_i ecef_pos;    /* pos ECEF in cm        */
   struct EcefCoor_i ecef_vel;    /* speed ECEF in cm/s    */
-  int16_t pressure_absolute;
+  int16_t pressure_absolute;     /* */
   uint8_t valid_sensors;
 };
 

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp       
2010-10-03 21:57:03 UTC (rev 6056)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp       
2010-10-04 10:04:18 UTC (rev 6057)
@@ -83,19 +83,25 @@
   spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, 
&crc_valid);
   
   struct AutopilotMessageVIUp *in = &msg_in.payload.msg_up; 
-  RATES_FLOAT_OF_BFP(imu_float.gyro, in->gyro);
-  ACCELS_FLOAT_OF_BFP(imu_float.accel, in->accel); 
+
+  if(in->valid_sensors & (1<< VI_IMU_DATA_VALID)){
+    RATES_FLOAT_OF_BFP(imu_float.gyro, in->gyro);
+    ACCELS_FLOAT_OF_BFP(imu_float.accel, in->accel); 
+  }
   
-  if(in->valid_sensors & MAG_DATA_VALID){
-         MAGS_FLOAT_OF_BFP(imu_float.mag, in->mag); 
+  if(in->valid_sensors & (1<< VI_MAG_DATA_VALID)){
+    MAGS_FLOAT_OF_BFP(imu_float.mag, in->mag); 
+    printf("MAG: %f %f %f\n", imu_float.mag.x, imu_float.mag.y, 
imu_float.mag.z);
   }
   
-  if(in->valid_sensors & GPS_DATA_VALID){
-               VECT3_COPY(imu_ecef_pos, in->ecef_pos);
-               printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, 
imu_ecef_pos.z);
-               VECT3_COPY(imu_ecef_vel, in->ecef_vel);
-       }
+  if(in->valid_sensors & (1<<VI_GPS_DATA_VALID)){
+    VECT3_COPY(imu_ecef_pos, in->ecef_pos);
+    printf("GPS: %d %d %d\n", imu_ecef_pos.x, imu_ecef_pos.y, imu_ecef_pos.z);
+    VECT3_COPY(imu_ecef_vel, in->ecef_vel);
+  }
+  
   return in->valid_sensors;
+
 }
 
 static void main_run_ins(uint8_t data_valid) {
@@ -107,7 +113,7 @@
   
   ins.predict(RATES_AS_VECTOR(imu_float.gyro), 
COORDS_AS_VECTOR(imu_float.accel), dt_imu_freq);
   
-  if(data_valid & MAG_DATA_VALID){
+  if(data_valid & VI_MAG_DATA_VALID){
                ins.obs_vector(reference_direction, 
COORDS_AS_VECTOR(imu_float.mag), mag_noise);
        }
   
@@ -116,7 +122,7 @@
                ins.obs_vector(ins.avg_state.position.normalized(), 
COORDS_AS_VECTOR(imu_float.accel), 0.027);
        }
   
-  if(data_valid & GPS_DATA_VALID){
+  if(data_valid & VI_GPS_DATA_VALID){
     const Vector3d gps_pos_noise = Vector3d::Ones()  *10*10;
     const Vector3d gps_speed_noise = Vector3d::Ones()*0.1*0.1;
                //ins.obs_gps_pv_report(COORDS_AS_VECTOR(imu_ecef_pos)/100, 
COORDS_AS_VECTOR(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise);

Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-10-03 
21:57:03 UTC (rev 6056)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-10-04 
10:04:18 UTC (rev 6057)
@@ -92,9 +92,10 @@
 extern void vi_periodic(void);
 extern void vi_update_info(void);
 
-extern void vi_notify_gps(void);
-extern void vi_notify_mag(void);
-extern void vi_notify_baro_abs(void);
+extern void vi_notify_imu_available(void);
+extern void vi_notify_mag_available(void);
+extern void vi_notify_gps_available(void);
+extern void vi_notify_baro_abs_available(void);
 
 /* must be implemented by specific module */
 extern void vi_impl_init(void);

Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_overo_link.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_overo_link.c      
2010-10-03 21:57:03 UTC (rev 6056)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_overo_link.c      
2010-10-04 10:04:18 UTC (rev 6057)
@@ -53,23 +53,26 @@
 void vi_overo_link_on_msg_received(void) {
 
   overo_link.up.msg.valid_sensors = vi.available_sensors;
-  
-  RATES_COPY(overo_link.up.msg.gyro, imu.gyro);
-  VECT3_COPY(overo_link.up.msg.accel, imu.accel);
 
-  if (vi.available_sensors && (1<<MAG_DATA_VALID)) {
+  if (vi.available_sensors & (1<<VI_IMU_DATA_VALID)) {
+    RATES_COPY(overo_link.up.msg.gyro, imu.gyro);
+    VECT3_COPY(overo_link.up.msg.accel, imu.accel);
+    vi.available_sensors &= ~(1<<VI_IMU_DATA_VALID);
+  }
+  if (vi.available_sensors & (1<<VI_MAG_DATA_VALID)) {
     VECT3_COPY(overo_link.up.msg.mag, imu.mag);
-    vi.available_sensors &= ~(1<<MAG_DATA_VALID);
+    vi.available_sensors &= ~(1<<VI_MAG_DATA_VALID);
   }
-  if (vi.available_sensors && (1<<GPS_DATA_VALID)) {
+  if (vi.available_sensors & (1<<VI_GPS_DATA_VALID)) {
     VECT3_COPY(overo_link.up.msg.ecef_pos, booz_gps_state.ecef_pos);
     VECT3_COPY(overo_link.up.msg.ecef_vel, booz_gps_state.ecef_vel);
-    vi.available_sensors &= ~(1<<GPS_DATA_VALID);
+    vi.available_sensors &= ~(1<<VI_GPS_DATA_VALID);
   }
-  if (vi.available_sensors && (1<<BARO_ABS_DATA_VALID)) {
+  if (vi.available_sensors & (1<<VI_BARO_ABS_DATA_VALID)) {
     overo_link.up.msg.pressure_absolute = baro.absolute;
-    vi.available_sensors &= ~(1<<BARO_ABS_DATA_VALID);
+    vi.available_sensors &= ~(1<<VI_BARO_ABS_DATA_VALID);
   }
+  
 }
 
 
@@ -79,14 +82,18 @@
 }
 
 
-void vi_notify_gps(void) {
-  vi.available_sensors |= (1<<GPS_DATA_VALID);
+void vi_notify_imu_available(void) {
+  vi.available_sensors |= (1<<VI_IMU_DATA_VALID);
 }
 
-void vi_notify_mag(void) {
-  vi.available_sensors |= (1<<MAG_DATA_VALID);
+void vi_notify_gps_available(void) {
+  vi.available_sensors |= (1<<VI_GPS_DATA_VALID);
 }
 
-void vi_notify_baro_abs(void) {
-  vi.available_sensors |= (1<<BARO_ABS_DATA_VALID);
+void vi_notify_mag_available(void) {
+  vi.available_sensors |= (1<<VI_MAG_DATA_VALID);
 }
+
+void vi_notify_baro_abs_available(void) {
+  vi.available_sensors |= (1<<VI_BARO_ABS_DATA_VALID);
+}