AP_NavEKF: fixed log replay code

also added BARO and ARSP messages

libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde

@@ -56,6 +56,8 @@ 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);
+static AP_Vehicle::FixedWing aparm;
+static AP_Airspeed airspeed(aparm);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
@@ -63,7 +65,7 @@ SITL sitl;
 
 static const NavEKF &NavEKF = ahrs.get_NavEKF();
 
-static LogReader LogReader(ins, barometer, compass, g_gps);
+static LogReader LogReader(ins, barometer, compass, g_gps, airspeed);
 
 static FILE *plotf;
 static FILE *plotf2;
@@ -89,6 +91,7 @@ void setup()
     ahrs.set_compass(&compass);
     ahrs.set_fly_forward(true);
     ahrs.set_wind_estimation(true);
+    ahrs.set_correct_centrifugal(true);
     
     barometer.init();
     barometer.setHIL(0);
@@ -131,6 +134,8 @@ void setup()
         }
     }
 
+    ::printf("InertialNav started\n");
+
     if (!ahrs.have_inertial_nav()) {
         ::printf("Failed to start NavEKF\n");
         exit(1);
@@ -141,6 +146,9 @@ static void read_sensors(uint8_t type)
 {
     if (type == LOG_GPS_MSG) {
         g_gps->update();
+        if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {
+            ahrs.estimate_wind();
+        }
     } else if (type == LOG_IMU_MSG) {
         ahrs.update();
         if (ahrs.get_home().lat != 0) {
@@ -149,13 +157,9 @@ static void read_sensors(uint8_t type)
     } 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();
-        if (!done_baro_init) {
-            done_baro_init = true;
-            barometer.update_calibration();
-        }
-    } else if (type == LOG_COPTER_CONTROL_TUNING_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) {
+    } else if (type == LOG_PLANE_AIRSPEED_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) {
+        ahrs.set_airspeed(&airspeed);
+    } else if (type == LOG_BARO_MSG) {
         barometer.read();
         if (!done_baro_init) {
             done_baro_init = true;
@@ -208,7 +212,7 @@ void loop()
             NavEKF.getMagXYZ(magXYZ);
             NavEKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
             NavEKF.getVariances(velVar, posVar, magVar, tasVar);
-            ekf_relpos = ahrs.get_relative_position_NED();
+            ahrs.get_relative_position_NED(ekf_relpos);
             Vector3f inav_pos = inertial_nav.get_position() * 0.01f;
             float temp = degrees(ekf_euler.z);
 

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp

@@ -18,13 +18,14 @@
 
 extern const AP_HAL::HAL& hal;
 
-LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps) :
+LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps, AP_Airspeed &_airspeed) :
     vehicle(VEHICLE_UNKNOWN),
     fd(-1),
     ins(_ins),
     baro(_baro),
     compass(_compass),
-    gps(_gps)
+    gps(_gps),
+    airspeed(_airspeed)
 {}
 
 bool LogReader::open_log(const char *logfile)
@@ -48,19 +49,6 @@ 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;
@@ -84,6 +72,14 @@ struct PACKED log_Plane_Attitude {
     uint16_t error_yaw;
 };
 
+struct PACKED log_AIRSPEED {
+    LOG_PACKET_HEADER;
+    uint32_t timestamp;
+    float   airspeed;
+    float   diffpressure;
+    int16_t temperature;
+};
+
 struct PACKED log_Copter_Attitude {
     LOG_PACKET_HEADER;
     int16_t control_roll;
@@ -94,19 +90,6 @@ struct PACKED log_Copter_Attitude {
     uint16_t yaw;
 };
 
-struct PACKED log_Copter_Control_Tuning {
-    LOG_PACKET_HEADER;
-    int16_t throttle_in;
-    int16_t sonar_alt;
-    int32_t baro_alt;
-    float   next_wp_alt;
-    int16_t desired_sonar_alt;
-    int16_t angle_boost;
-    int16_t climb_rate;
-    int16_t throttle_out;
-    int16_t desired_climb_rate;
-};
-
 struct PACKED log_Copter_INAV {
     LOG_PACKET_HEADER;
     int16_t baro_alt;
@@ -149,17 +132,15 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
         break;
     }
 
-    case LOG_PLANE_NTUN_MSG: {
-        struct log_Plane_Nav_Tuning msg;
+    case LOG_PLANE_AIRSPEED_MSG: {
+        struct log_AIRSPEED msg;
         if(sizeof(msg) != length) {
-            printf("Bad NAV_TUNING length\n");
+            printf("Bad AIRSPEED 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);
-        }
+        wait_timestamp(msg.timestamp);
+        airspeed.setHIL(msg.airspeed, msg.diffpressure, msg.temperature);
         break;
     }
     }
@@ -193,20 +174,6 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
         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) {
@@ -328,6 +295,18 @@ bool LogReader::update(uint8_t &type)
         break;
     }
 
+    case LOG_BARO_MSG: {
+        struct log_BARO msg;
+        if(sizeof(msg) != f.length) {
+            printf("Bad LOG_BARO length\n");
+            exit(1);
+        }
+        memcpy(&msg, data, sizeof(msg));
+        wait_timestamp(msg.timestamp);
+        baro.setHIL(msg.pressure, msg.temperature*0.01f);
+        break;
+    }
+
     default:
         if (vehicle == VEHICLE_PLANE) {
             process_plane(f.type, data, f.length);

libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h

@@ -1,14 +1,13 @@
 
 enum log_messages {
     // plane specific messages
-    LOG_PLANE_NTUN_MSG     = 2,
     LOG_PLANE_ATTITUDE_MSG = 10,
     LOG_PLANE_COMPASS_MSG  = 12,
+    LOG_PLANE_AIRSPEED_MSG  = 18,
 
     // copter specific messages
     LOG_COPTER_ATTITUDE_MSG = 1,
     LOG_COPTER_COMPASS_MSG  = 15,
-    LOG_COPTER_CONTROL_TUNING_MSG = 4,
     LOG_COPTER_INAV_MSG = 17,
 };
 
@@ -16,7 +15,7 @@ enum log_messages {
 class LogReader 
 {
 public:
-    LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps);
+    LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps, AP_Airspeed &_airspeed);
     bool open_log(const char *logfile);
     bool update(uint8_t &type);
     bool wait_type(uint8_t type);
@@ -35,6 +34,7 @@ private:
     AP_Baro_HIL &baro;
     AP_Compass_HIL &compass;
     GPS *&gps;
+    AP_Airspeed &airspeed;
 
     uint32_t ground_alt_cm;
 

libraries/AP_NavEKF/examples/AP_NavEKF/plotit.sh

@@ -20,7 +20,7 @@ cat <<EOF > _plot.gnu
 set style data lines
 set xlabel "time(s)"
 $cmd
-
+pause -1 "hit return to exit"
 EOF
 gnuplot _plot.gnu