AP_NavEKF: handle intertial nav

libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde

@@ -34,6 +34,7 @@
 #include <DataFlash.h>
 #include <GCS_MAVLink.h>
 #include <AP_GPS.h>
+#include <AP_GPS_Glitch.h>
 #include <AP_AHRS.h>
 #include <SITL.h>
 #include <AP_Compass.h>
@@ -53,6 +54,8 @@ static AP_GPS_HIL gps_driver;
 static GPS *g_gps = &gps_driver;
 static AP_Compass_HIL compass;
 static AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
+static GPS_Glitch gps_glitch(g_gps);
+static AP_InertialNav inertial_nav(ahrs, barometer, g_gps, gps_glitch);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
@@ -83,12 +86,16 @@ void setup()
     ahrs.set_wind_estimation(true);
     
     barometer.init();
+    barometer.setHIL(0);
+    barometer.read();
+    barometer.update_calibration();
     compass.init();
+    inertial_nav.init();
     ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
 
     plotf = fopen("plot.dat", "w");
     plotf2 = fopen("plot2.dat", "w");
-    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
+    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
     fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ AX AY AZ MN ME MD MX MY MZ E1ref E2ref E3ref\n");
 
     ahrs.set_ekf_use(true);
@@ -100,14 +107,16 @@ void setup()
         read_sensors(type);
         if (type == LOG_GPS_MSG && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
             gps_fix_count++;
-            if (gps_fix_count == 2) {
+            if (gps_fix_count == 10) {
-                ::printf("GPS Lock at %.7f %.7f %.2fm\n", 
+                ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", 
                          g_gps->latitude*1.0e-7f, 
                          g_gps->longitude*1.0e-7f,
-                         g_gps->altitude_cm*0.01f);
+                         g_gps->altitude_cm*0.01f,
+                         hal.scheduler->millis()*0.001f);
                 ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
-                barometer.calibrate();
+                barometer.update_calibration();
-                compass.set_initial_location(g_gps->latitude, g_gps->longitude);            
+                compass.set_initial_location(g_gps->latitude, g_gps->longitude);
+                inertial_nav.setup_home_position();
             }
         }
     }
@@ -122,12 +131,18 @@ static void read_sensors(uint8_t type)
 {
     if (type == LOG_GPS_MSG) {
         g_gps->update();
-        barometer.read();
     } else if (type == LOG_IMU_MSG) {
         ahrs.update();
+        if (ahrs.get_home().lat != 0) {
+            inertial_nav.update(ins.get_delta_time());
+        }
     } else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
                (type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
         compass.read();
+    } else if (type == LOG_PLANE_NTUN_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) {
+        barometer.read();
+    } else if (type == LOG_COPTER_CONTROL_TUNING_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) {
+        barometer.read();
     }
 }
 
@@ -136,7 +151,7 @@ void loop()
     while (true) {
         uint8_t type;
         if (!LogReader.update(type)) {
-            ::printf("End of log\n");
+            ::printf("End of log at %.1f seconds\n", hal.scheduler->millis()*0.001f);
             fclose(plotf);
             exit(0);
         }
@@ -152,6 +167,7 @@ void loop()
             Vector3f magNED;
             Vector3f magXYZ;
             Vector3f DCM_attitude;
+            Vector3f ekf_relpos;
             ahrs.get_secondary_attitude(DCM_attitude);
             NavEKF.getEulerAngles(ekf_euler);
             NavEKF.getVelNED(velNED);
@@ -160,23 +176,35 @@ void loop()
             NavEKF.getAccelBias(accelBias);
             NavEKF.getMagNED(magNED);
             NavEKF.getMagXYZ(magXYZ);
+            ekf_relpos = ahrs.get_relative_position_NED();
+            Vector3f inav_pos = inertial_nav.get_position() * 0.01f;
             float temp = degrees(ekf_euler.z);
             if (temp < 0.0f) temp = temp + 360.0f;
-            fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
+            fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
                     hal.scheduler->millis() * 0.001f,
                     LogReader.get_sim_attitude().x,
                     LogReader.get_sim_attitude().y,
                     LogReader.get_sim_attitude().z,
+                    barometer.get_altitude(),
                     LogReader.get_attitude().x,
                     LogReader.get_attitude().y,
                     LogReader.get_attitude().z,
+                    LogReader.get_inavpos().x,
+                    LogReader.get_inavpos().y,
+                    LogReader.get_inavpos().z,
                     degrees(DCM_attitude.x),
                     degrees(DCM_attitude.y),
                     degrees(DCM_attitude.z),
                     degrees(ekf_euler.x),
                     degrees(ekf_euler.y),
-                    degrees(ekf_euler.z));
+                    degrees(ekf_euler.z),
-            fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.4f %.4f %.1f %.1f %.1f \n",
+                    inav_pos.x,
+                    inav_pos.y,
+                    inav_pos.z,
+                    ekf_relpos.x,
+                    ekf_relpos.y,
+                    -ekf_relpos.z);
+            fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.4f %.4f %.1f %.1f %.1f\n",
                     hal.scheduler->millis() * 0.001f,
                     degrees(ekf_euler.x),
                     degrees(ekf_euler.y),

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp

@@ -50,6 +50,19 @@ struct PACKED log_Plane_Compass {
     int16_t offset_z;
 };
 
+struct PACKED log_Plane_Nav_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t yaw;
+    uint32_t wp_distance;
+    uint16_t target_bearing_cd;
+    uint16_t nav_bearing_cd;
+    int16_t altitude_error_cm;
+    int16_t airspeed_cm;
+    float   altitude;
+    uint32_t groundspeed_cm;
+};
+
 struct PACKED log_Copter_Compass {
     LOG_PACKET_HEADER;
     int16_t mag_x;
@@ -96,6 +109,20 @@ struct PACKED log_Copter_Control_Tuning {
     int16_t desired_climb_rate;
 };
 
+struct PACKED log_Copter_INAV {
+    LOG_PACKET_HEADER;
+    int16_t baro_alt;
+    int16_t inav_alt;
+    int16_t inav_climb_rate;
+    float   accel_corr_x;
+    float   accel_corr_y;
+    float   accel_corr_z;
+    int32_t gps_lat_from_home;
+    int32_t gps_lon_from_home;
+    float   inav_lat_from_home;
+    float   inav_lon_from_home;
+};
+
 void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
 {
     switch (type) {
@@ -123,6 +150,20 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
         attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
         break;
     }
+
+    case LOG_PLANE_NTUN_MSG: {
+        struct log_Plane_Nav_Tuning msg;
+        if(sizeof(msg) != length) {
+            printf("Bad NAV_TUNING length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.time_ms);
+        if (ground_alt_cm != 0) {
+            baro.setHIL(ground_alt_cm*0.01f + msg.altitude);
+        }
+        break;
+    }
     }
 }
 
@@ -153,6 +194,34 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
         attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
         break;
     }
+
+    case LOG_COPTER_CONTROL_TUNING_MSG: {
+        struct log_Copter_Control_Tuning msg;
+        if(sizeof(msg) != length) {
+            printf("Bad CONTROL_TUNING length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        //wait_timestamp(msg.time_ms);
+        if (ground_alt_cm != 0) {
+            baro.setHIL(0.01f * (ground_alt_cm + msg.baro_alt));
+        }
+        break;
+    }
+
+    case LOG_COPTER_INAV_MSG: {
+        struct log_Copter_INAV msg;
+        if(sizeof(msg) != length) {
+            printf("Bad INAV length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        inavpos = Vector3f(msg.inav_lat_from_home * LATLON_TO_CM * 0.01f, 
+                           msg.inav_lon_from_home * LATLON_TO_CM * 0.01f * 
+                           cosf(gps->latitude * 1.0e-7f * DEG_TO_RAD),
+                           msg.inav_alt*0.01f);
+        break;
+    }
     }
 }
 
@@ -246,7 +315,6 @@ bool LogReader::update(uint8_t &type)
         if (msg.status == 3 && ground_alt_cm == 0) {
             ground_alt_cm = msg.altitude;
         }
-        baro.setHIL((ground_alt_cm + msg.rel_altitude)*1.0e-2f);
         break;
     }
 

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h

@@ -8,7 +8,8 @@ enum log_messages {
     // copter specific messages
     LOG_COPTER_ATTITUDE_MSG = 1,
     LOG_COPTER_COMPASS_MSG  = 15,
-    LOG_COPTER_CONTROL_TUNING_MSG = 4
+    LOG_COPTER_CONTROL_TUNING_MSG = 4,
+    LOG_COPTER_INAV_MSG = 17,
 };
 
 
@@ -19,7 +20,9 @@ public:
     bool open_log(const char *logfile);
     bool update(uint8_t &type);
     bool wait_type(uint8_t type);
+
     const Vector3f &get_attitude(void) const { return attitude; }
+    const Vector3f &get_inavpos(void) const { return inavpos; }
     const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
 
     enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
@@ -40,6 +43,7 @@ private:
 
     Vector3f attitude;
     Vector3f sim_attitude;
+    Vector3f inavpos;
 
     void wait_timestamp(uint32_t timestamp);
 

libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu

@@ -1,3 +1,3 @@
 set style data lines
-plot "plot.dat" using "SIM.Roll", "" using "DCM.Roll", "" using "EKF.Roll"
+plot 'plot.dat' using "SIM.Roll", "" using "DCM.Roll", "" using "EKF.Roll"
 pause -1 "hit return to exit"