GPS fixes in dev branch...

paparazzi · May 10, 2011 · 1d8f7f5 · 1d8f7f5
1 parent 129f3b6
commit 1d8f7f5
Show file tree

Hide file tree

Showing 6 changed files with 56 additions and 24 deletions.
diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -9,8 +9,8 @@
   <servos>
     <servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/>
     <servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/>
-    <servo name="AILERON_RIGHT" no="8" min="2000" neutral="1500" max="1000"/>
-    <servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="AILERON_RIGHT" no="1" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="8" min="2000" neutral="1500" max="1000"/>
     <servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/>
   </servos>
 
@@ -64,7 +64,7 @@
     <define name="GYRO_Q_SIGN" value="1"/>
     <define name="GYRO_R_SIGN" value="1"/>
 
-    <define name="ACCEL_X_NEUTRAL" value="0"/>
+    <define name="ACCEL_X_NEUTRAL" value="-14"/>
     <define name="ACCEL_Y_NEUTRAL" value="0"/>
     <define name="ACCEL_Z_NEUTRAL" value="0"/>
 
@@ -225,6 +225,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
+      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
 
       <configure name="AHRS_ALIGNER_LED" value="3"/>
       <configure name="CPU_LED" value="3"/>
@@ -237,7 +238,9 @@
 
     <!-- Sensors -->
     <!--
-    <subsystem name="ahrs" type="ic"/>
+    <subsystem name="ahrs" type="ic">
+      <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
+    </subsystem>
     <subsystem name="imu" type="aspirin_i2c"/>
     -->
     <subsystem name="imu" type="ppzuav"/>

diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
@@ -188,7 +188,7 @@
 #define PERIODIC_SEND_GPS(_chan) {                                      \
     static uint8_t i;                                                   \
     int16_t climb = -gps.ned_vel.z;                                     \
-    int16_t course = DegOfRad(gps.course / 10);                        \
+    int16_t course = DegOfRad(estimator_hspeed_dir * 10);                        \
     DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
     if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
     if (i >= gps.nb_channels * 2) i = 0;                                    \

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -410,6 +410,12 @@ static inline void attitude_loop( void ) {
 
 }
 
+#ifdef USE_IMU
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+volatile uint8_t new_ins_attitude = 0;
+#endif
+#endif
+
 void periodic_task_ap( void ) {
 
   static uint8_t _60Hz = 0;
@@ -721,6 +727,10 @@ static inline void on_gyro_event( void ) {
     LED_OFF(AHRS_CPU_LED);
 #endif
 
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+  new_ins_attitude = 1;
+#endif
+
 }
 
 static inline void on_mag_event(void)

diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c
@@ -68,9 +68,10 @@ void imu_impl_init(void)
 #if PERIODIC_FREQUENCY == 60
   /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
+#  warning ITG3200 read at 50Hz
 #else
 #  if PERIODIC_FREQUENCY == 120
-#  warning ITG3200 read at 120Hz
+#  warning ITG3200 read at 100Hz
   /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
   ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
 #  else
@@ -189,7 +190,7 @@ void imu_periodic( void )
 #if PERIODIC_FREQUENCY > 60
   });
 #endif
-  RunOnceEvery(10,ppzuavimu_module_downlink_raw());
+  //RunOnceEvery(10,ppzuavimu_module_downlink_raw());
 }
 
 void ppzuavimu_module_downlink_raw( void )

diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
@@ -176,8 +176,6 @@ int32_t xsens_nanosec;
 int16_t xsens_year;
 int8_t xsens_month;
 int8_t xsens_day;
-float xsens_lat;
-float xsens_lon;
 
 static uint8_t xsens_id;
 static uint8_t xsens_status;
@@ -223,18 +221,35 @@ void ins_periodic_task( void ) {
 #include "estimator.h"
 
 void handle_ins_msg( void) {
+
+
+  // Send to Estimator (Control)
   EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
   EstimatorSetRate(ins_p,ins_q);
+
+  // Position
+  float gps_east = gps.utm_pos.east / 100.;
+  float gps_north = gps.utm_pos.north / 100.;
+  gps_east -= nav_utm_east0;
+  gps_north -= nav_utm_north0;
+  EstimatorSetPosXY(gps_east, gps_north);
+
+  // Altitude and vertical speed
+  EstimatorSetAlt(-ins_z);
+
+  // Horizontal speed
+  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
   if (gps.fix != GPS_FIX_3D)
-    gps.gspeed = 0;
+    fspeed = 0;
+  float fclimb = -ins_vz;
+  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
+  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 
-  gps.course = (atan2f((float)ins_vx, (float)ins_vy))*1e7;
-  gps.ned_vel.z = (int16_t)(ins_vz * 100);
-  gps.gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
 
-  EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta);
-  // EstimatorSetAlt(ins_z);
-  estimator_update_state_gps();
+  // Now also finish filling the gps struct for telemetry purposes
+  gps.gspeed = fspeed * 100.;
+  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
+  gps.course = fcourse * 1e7;
   // reset_gps_watchdog();
 }
 
@@ -302,15 +317,20 @@ void parse_ins_msg( void ) {
         ins_x = utm_f.east;
         ins_y = utm_f.north;
 
-        gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 100;
-        ins_z = -(INS_FORMAT)gps.hmsl / 1000.;
-        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
-        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
+        // Altitude: FIXME Xsens gives ellipsoid height and not geoid height
+        ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
+        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
+        gps.lla_pos.alt = gps.hmsl;
+        gps.utm_pos.alt = gps.hmsl;
+        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
+        ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
         ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
-        gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
+        gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
+        gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
+        gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
         gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
         gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
-        gps.pdop = 5;
+        gps.pdop = 5;  // FIXME Not output by XSens
 #endif
         offset += XSENS_DATA_RAWGPS_LENGTH;
       }

diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
@@ -38,8 +38,6 @@ extern int32_t xsens_nanosec;
 extern int16_t xsens_year;
 extern int8_t xsens_month;
 extern int8_t xsens_day;
-extern float xsens_lat;
-extern float xsens_lon;
 extern uint8_t xsens_msg_status;
 extern uint16_t xsens_time_stamp;