gps-timeout fixed

conf/airframes/TU_Delft/skywalker.xml

@@ -8,8 +8,8 @@
 
   <servos>
     <servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
-    <servo name="AILERON_LEFT" no="2" min="1100" neutral="1500" max="1900"/>
+    <servo name="AILERON_LEFT" no="2" min="900" neutral="1500" max="2100"/>
-    <servo name="AILERON_RIGHT" no="6" min="1100" neutral="1500" max="1900"/>
+    <servo name="AILERON_RIGHT" no="6" min="900" neutral="1500" max="2100"/>
     <servo name="ELEVATOR" no="7" min="1900" neutral="1500" max="1100"/>
     <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
   </servos>
@@ -31,7 +31,7 @@
   </rc_commands>
 
   <section name="SERVO_MIXER_GAINS">
-    <define name="AILERON_NEUTRAL" value="0.45f"/>
+    <define name="AILERON_NEUTRAL" value="0.3f"/>
 
     <define name="AILERON_RATE_UP" value="1.0f"/>
     <define name="AILERON_RATE_DOWN" value="0.5f"/>
@@ -67,7 +67,7 @@
     <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
     <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
 
-    <set servo="RUDDER" value="@YAW"/>
+    <set servo="RUDDER" value="@YAW + @ROLL * 0.3"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
 
     <!-- Pitch with Brake-Trim Function -->
@@ -75,12 +75,12 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_ROLL" value="RadOfDeg(75)"/>
-    <define name="MAX_PITCH" value="0.7"/>
+    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
   </section>
 
   <section name="BAT">
-    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+<!--    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/> -->
 
     <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
     <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
@@ -110,54 +110,56 @@
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
     <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
     <!-- outer loop proportional gain -->
-    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <define name="ALTITUDE_PGAIN" value="-0.108000002801"/>
     <!-- outer loop saturation -->
     <define name="ALTITUDE_MAX_CLIMB" value="2."/>
 
     <!-- auto throttle inner loop -->
-    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.57800000906"/>
     <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
     <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
     <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
     <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
-    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.108999997377" unit="%/(m/s)"/>
-    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="0."/>
-    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0."/>
-    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.324999988079"/>
 
     <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
   </section>
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="-1.20000004768"/>
-    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_DGAIN" value="0.300000011921"/>
-    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="1.01600003242"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
-    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_PGAIN" value="-13476.5615234"/>
-    <define name="PITCH_DGAIN" value="1.5"/>
+    <define name="PITCH_DGAIN" value="7.73400020599"/>
 
-    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+    <define name="ELEVATOR_OF_ROLL" value="3007.81298828"/>
 
     <define name="ROLL_SLEW" value="1."/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <define name="ROLL_ATTITUDE_GAIN" value="-11718.75"/>
-    <define name="ROLL_RATE_GAIN" value="0."/>
+    <define name="ROLL_RATE_GAIN" value="-820.312011719"/>
   </section>
 
+<!--
   <section name="AGGRESSIVE" prefix="AGR_">
-    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_START" value="20"/>
-    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/>
-    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_THROTTLE" value="1.00"/>
-    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/>
-    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_THROTTLE" value="0.1"/>
-    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/>
-    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/>
     <define name="DESCENT_NAV_RATIO" value="1.0"/>
   </section>
+-->
 
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
@@ -180,19 +182,19 @@
       <configure name="XSENS_UART_NR" value="0"/>
     </load>
 
-<!--    <load name="light.xml">
+    <load name="light.xml">
       <define name="LIGHT_LED_STROBE" value="3"/>
       <define name="LIGHT_LED_NAV" value="2"/>
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
 
-    <load name="digital_cam_i2c.xml"/>  -->
+<!--    <load name="digital_cam_i2c.xml"/>  -->
-  <!--    <load name="ins_ppzuavimu.xml"/>  
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
-    <load name="digital_cam.xml" >
+    <load name="digital_cam.xml">
       <define name="DC_SHUTTER_LED" value="2"/>
     </load>
--->
+
   </modules>
 
   <firmware name="fixedwing">
@@ -202,7 +204,10 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
+<!--
       <define name="ALT_KALMAN"/>
+      <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+-->
     </target>
     <target name="sim" board="pc"/>
 
@@ -219,9 +224,7 @@
     <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
-<!--
     <subsystem name="i2c"/>
--->
 
 </firmware>
 

sw/airborne/modules/ins/ins_xsens.c

@@ -32,6 +32,7 @@
 
 #include "generated/airframe.h"
 
+#include "sys_time.h"
 #include "downlink.h"
 #include "messages.h"
 
@@ -225,7 +226,7 @@ void handle_ins_msg( void) {
 
 
   // Send to Estimator (Control)
-  EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
+  EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
   EstimatorSetRate(ins_p,ins_q);
 
   // Position
@@ -236,13 +237,17 @@ void handle_ins_msg( void) {
   EstimatorSetPosXY(gps_east, gps_north);
 
   // Altitude and vertical speed
-  EstimatorSetAlt(-ins_z);
+  float hmsl = gps.hmsl;
+  hmsl /= 1000.0f;
+  EstimatorSetAlt(hmsl);
 
   // Horizontal speed
   float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
   if (gps.fix != GPS_FIX_3D)
+  {
     fspeed = 0;
-  float fclimb = -ins_vz;
+  }
+  float fclimb = -ins_vz * ;
   float fcourse = atan2f((float)ins_vy, (float)ins_vx);
   EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 
@@ -251,7 +256,6 @@ void handle_ins_msg( void) {
   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();
 }
 
 void parse_ins_msg( void ) {
@@ -270,6 +274,8 @@ void parse_ins_msg( void ) {
     gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
     gps.num_sv = 0;
 
+    gps.last_fix_time = cpu_time_sec;
+
     uint8_t i;
     // Do not write outside buffer
     for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
@@ -297,7 +303,10 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
 #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
-    LED_TOGGLE(3);
+#ifdef GPS_LED
+    LED_TOGGLE(GPS_LED);
+#endif
+        gps.last_fix_time = cpu_time_sec;
         gps.week = 0; // FIXME
         gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
         gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
@@ -326,11 +335,11 @@ void parse_ins_msg( void ) {
 	// Compute geoid (MSL) height
         float hmsl;
 	WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
-        gps.hmsl =  (hmsl * 1000.0f);
+        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
 
-        ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
+        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_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.;
+        ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
         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);
@@ -405,6 +414,8 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_Position(xsens_output_mode)) {
 #if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
+        gps.last_fix_time = cpu_time_sec;
+
         lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
         lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
         gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);