diff --git a/Tools/CPUInfo/make.inc b/Tools/CPUInfo/make.inc index 742c4709c3..c611640fcf 100644 --- a/Tools/CPUInfo/make.inc +++ b/Tools/CPUInfo/make.inc @@ -10,7 +10,8 @@ LIBRARIES += AP_GPS LIBRARIES += AP_InertialSensor LIBRARIES += AP_Math LIBRARIES += AP_Mission -LIBRARIES += AP_NavEKF +LIBRARIES += AP_NavEKF2 +LIBRARIES += AP_NavEKF3 LIBRARIES += AP_Notify LIBRARIES += AP_OpticalFlow LIBRARIES += AP_Param diff --git a/Tools/Hello/make.inc b/Tools/Hello/make.inc index 294358e1ab..786da96f87 100644 --- a/Tools/Hello/make.inc +++ b/Tools/Hello/make.inc @@ -14,7 +14,8 @@ LIBRARIES += AP_Vehicle LIBRARIES += DataFlash LIBRARIES += Filter LIBRARIES += GCS_MAVLink -LIBRARIES += AP_NavEKF +LIBRARIES += AP_NavEKF2 +LIBRARIES += AP_NavEKF3 LIBRARIES += AP_RangeFinder LIBRARIES += AP_Mission LIBRARIES += AP_Rally diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h index 4a89c73ae2..af4c263d88 100644 --- a/Tools/Replay/Parameters.h +++ b/Tools/Replay/Parameters.h @@ -12,10 +12,10 @@ public: k_param_ins, k_param_ahrs, k_param_airspeed, - k_param_NavEKF, k_param_NavEKF2, k_param_compass, - k_param_dataflash + k_param_dataflash, + k_param_NavEKF3 }; AP_Int8 dummy; }; diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index a28e005ffe..d865c43735 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -64,10 +64,6 @@ const AP_Param::Info ReplayVehicle::var_info[] = { // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp GOBJECT(airspeed, "ARSPD_", AP_Airspeed), - // @Group: EKF_ - // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp - GOBJECTN(EKF, NavEKF, "EKF_", NavEKF), - // @Group: EK2_ // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2), @@ -80,6 +76,10 @@ const AP_Param::Info ReplayVehicle::var_info[] = { // @Path: ../libraries/DataFlash/DataFlash.cpp GOBJECT(dataflash, "LOG", DataFlash_Class), + // @Group: EK3_ + // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp + GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3), + AP_VAREND }; @@ -92,6 +92,9 @@ void ReplayVehicle::load_parameters(void) } AP_Param::set_default_by_name("EKF_ENABLE", 1); AP_Param::set_default_by_name("EK2_ENABLE", 1); + AP_Param::set_default_by_name("EK2_IMU_MASK", 1); + AP_Param::set_default_by_name("EK3_ENABLE", 1); + AP_Param::set_default_by_name("EK3_IMU_MASK", 1); AP_Param::set_default_by_name("LOG_REPLAY", 1); AP_Param::set_default_by_name("AHRS_EKF_TYPE", 2); AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60); @@ -122,7 +125,8 @@ void ReplayVehicle::setup(void) ahrs.set_ekf_use(true); EKF2.set_enable(true); - + EKF3.set_enable(true); + printf("Starting disarmed\n"); hal.util->set_soft_armed(false); @@ -541,20 +545,6 @@ void Replay::setup() } set_ins_update_rate(log_info.update_rate); - - plotf = xfopen("plot.dat", "we"); - plotf2 = xfopen("plot2.dat", "we"); - ekf1f = xfopen("EKF1.dat", "we"); - ekf2f = xfopen("EKF2.dat", "we"); - ekf3f = xfopen("EKF3.dat", "we"); - ekf4f = xfopen("EKF4.dat", "we"); - - fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE AHR2.Roll AHR2.Pitch AHR2.Yaw 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 WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n"); - fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); - fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); - fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n"); - fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n"); } void Replay::set_ins_update_rate(uint16_t _update_rate) { @@ -624,7 +614,7 @@ void Replay::set_signal_handlers(void) void Replay::write_ekf_logs(void) { if (!LogReader::in_list("EKF", nottypes)) { - _vehicle.dataflash.Log_Write_EKF2(_vehicle.ahrs,false); + _vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false); } if (!LogReader::in_list("AHRS2", nottypes)) { _vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs); @@ -719,7 +709,7 @@ void Replay::read_sensors(const char *type) } } - if (logmatch && streq(type, "NKF1")) { + if (logmatch && (streq(type, "NKF1") || streq(type, "XKF1"))) { write_ekf_logs(); } } @@ -734,9 +724,9 @@ void Replay::log_check_generate(void) Vector3f velocity; Location loc {}; - _vehicle.EKF.getEulerAngles(euler); - _vehicle.EKF.getVelNED(velocity); - _vehicle.EKF.getLLH(loc); + _vehicle.EKF2.getEulerAngles(-1,euler); + _vehicle.EKF2.getVelNED(-1,velocity); + _vehicle.EKF2.getLLH(loc); struct log_Chek packet = { LOG_PACKET_HEADER_INIT(LOG_CHEK_MSG), @@ -766,9 +756,9 @@ void Replay::log_check_solution(void) Vector3f velocity; Location loc {}; - _vehicle.EKF.getEulerAngles(euler); - _vehicle.EKF.getVelNED(velocity); - _vehicle.EKF.getLLH(loc); + _vehicle.EKF2.getEulerAngles(-1,euler); + _vehicle.EKF2.getVelNED(-1,velocity); + _vehicle.EKF2.getLLH(loc); float roll_error = degrees(fabsf(euler.x - check_state.euler.x)); float pitch_error = degrees(fabsf(euler.y - check_state.euler.y)); @@ -826,212 +816,6 @@ void Replay::loop() return; } - Vector3f ekf_euler; - Vector3f velNED; - Vector2f posNE; - float posD; - Vector3f gyroBias; - float accelWeighting; - float accelZBias1; - float accelZBias2; - Vector3f windVel; - Vector3f magNED; - Vector3f magXYZ; - Vector3f DCM_attitude; - Vector3f velInnov; - Vector3f posInnov; - Vector3f magInnov; - float tasInnov; - float velVar; - float posVar; - float hgtVar; - Vector3f magVar; - float tasVar; - Vector2f offset; - uint16_t faultStatus; - - const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned(); - dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); - _vehicle.EKF.getEulerAngles(ekf_euler); - _vehicle.EKF.getVelNED(velNED); - _vehicle.EKF.getPosNE(posNE); - _vehicle.EKF.getPosD(posD); - _vehicle.EKF.getGyroBias(gyroBias); - _vehicle.EKF.getIMU1Weighting(accelWeighting); - _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2); - _vehicle.EKF.getWind(windVel); - _vehicle.EKF.getMagNED(magNED); - _vehicle.EKF.getMagXYZ(magXYZ); - _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); - _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - _vehicle.EKF.getFilterFaults(faultStatus); - Vector3f inav_pos = _vehicle.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 %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", - AP_HAL::millis() * 0.001f, - logreader.get_sim_attitude().x, - logreader.get_sim_attitude().y, - logreader.get_sim_attitude().z, - _vehicle.barometer.get_altitude(), - logreader.get_attitude().x, - logreader.get_attitude().y, - wrap_180_cd(logreader.get_attitude().z*100)*0.01f, - logreader.get_inavpos().x, - logreader.get_inavpos().y, - logreader.get_ahr2_attitude().x, - logreader.get_ahr2_attitude().y, - wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, - degrees(DCM_attitude.x), - degrees(DCM_attitude.y), - degrees(DCM_attitude.z), - degrees(ekf_euler.x), - degrees(ekf_euler.y), - degrees(ekf_euler.z), - inav_pos.x, - inav_pos.y, - inav_pos.z, - posNE.x, - posNE.y, - -posD); - fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", - AP_HAL::millis() * 0.001f, - degrees(ekf_euler.x), - degrees(ekf_euler.y), - temp, - velNED.x, - velNED.y, - velNED.z, - posNE.x, - posNE.y, - posD, - 60*degrees(gyroBias.x), - 60*degrees(gyroBias.y), - 60*degrees(gyroBias.z), - windVel.x, - windVel.y, - magNED.x, - magNED.y, - magNED.z, - magXYZ.x, - magXYZ.y, - magXYZ.z, - logreader.get_attitude().x, - logreader.get_attitude().y, - logreader.get_attitude().z); - - // define messages for EKF1 data packet - int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg) - int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg) - uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg) - float velN = (float)(velNED.x); // velocity North (m/s) - float velE = (float)(velNED.y); // velocity East (m/s) - float velD = (float)(velNED.z); // velocity Down (m/s) - float posN = (float)(posNE.x); // metres North - float posE = (float)(posNE.y); // metres East - float gyrX = (float)(6000*degrees(gyroBias.x)); // centi-deg/min - float gyrY = (float)(6000*degrees(gyroBias.y)); // centi-deg/min - float gyrZ = (float)(6000*degrees(gyroBias.z)); // centi-deg/min - - // print EKF1 data packet - fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n", - AP_HAL::millis() * 0.001f, - AP_HAL::millis(), - roll, - pitch, - yaw, - velN, - velE, - velD, - posN, - posE, - posD, - gyrX, - gyrY, - gyrZ); - - // define messages for EKF2 data packet - int8_t accWeight = (int8_t)(100*accelWeighting); - int8_t acc1 = (int8_t)(100*accelZBias1); - int8_t acc2 = (int8_t)(100*accelZBias2); - int16_t windN = (int16_t)(100*windVel.x); - int16_t windE = (int16_t)(100*windVel.y); - int16_t magN = (int16_t)(magNED.x); - int16_t magE = (int16_t)(magNED.y); - int16_t magD = (int16_t)(magNED.z); - int16_t magX = (int16_t)(magXYZ.x); - int16_t magY = (int16_t)(magXYZ.y); - int16_t magZ = (int16_t)(magXYZ.z); - - // print EKF2 data packet - fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n", - AP_HAL::millis() * 0.001f, - AP_HAL::millis(), - accWeight, - acc1, - acc2, - windN, - windE, - magN, - magE, - magD, - magX, - magY, - magZ); - - // define messages for EKF3 data packet - int16_t innovVN = (int16_t)(100*velInnov.x); - int16_t innovVE = (int16_t)(100*velInnov.y); - int16_t innovVD = (int16_t)(100*velInnov.z); - int16_t innovPN = (int16_t)(100*posInnov.x); - int16_t innovPE = (int16_t)(100*posInnov.y); - int16_t innovPD = (int16_t)(100*posInnov.z); - int16_t innovMX = (int16_t)(magInnov.x); - int16_t innovMY = (int16_t)(magInnov.y); - int16_t innovMZ = (int16_t)(magInnov.z); - int16_t innovVT = (int16_t)(100*tasInnov); - - // print EKF3 data packet - fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n", - AP_HAL::millis() * 0.001f, - AP_HAL::millis(), - innovVN, - innovVE, - innovVD, - innovPN, - innovPE, - innovPD, - innovMX, - innovMY, - innovMZ, - innovVT); - - // define messages for EKF4 data packet - int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); - int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); - int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); - int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); - int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); - int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); - int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); - int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); - int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); - - // print EKF4 data packet - fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n", - AP_HAL::millis() * 0.001f, - (unsigned)AP_HAL::millis(), - (int)sqrtvarV, - (int)sqrtvarP, - (int)sqrtvarH, - (int)sqrtvarMX, - (int)sqrtvarMY, - (int)sqrtvarMZ, - (int)sqrtvarVT, - (int)offsetNorth, - (int)offsetEast, - (int)faultStatus); } diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index 29f49c2411..1a3454c9f8 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -36,8 +36,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -63,9 +63,9 @@ public: Compass compass; AP_SerialManager serial_manager; RangeFinder rng {serial_manager}; - NavEKF EKF{&ahrs, barometer, rng}; NavEKF2 EKF2{&ahrs, barometer, rng}; - AP_AHRS_NavEKF ahrs {ins, barometer, gps, rng, EKF, EKF2}; + NavEKF3 EKF3{&ahrs, barometer, rng}; + AP_AHRS_NavEKF ahrs {ins, barometer, gps, rng, EKF2, EKF3}; AP_InertialNav_NavEKF inertial_nav{ahrs}; AP_Vehicle::FixedWing aparm; AP_Airspeed airspeed; diff --git a/Tools/Replay/make.inc b/Tools/Replay/make.inc index 49c2469fa0..9e759aa617 100644 --- a/Tools/Replay/make.inc +++ b/Tools/Replay/make.inc @@ -18,8 +18,8 @@ LIBRARIES += AP_Compass LIBRARIES += AP_Baro LIBRARIES += AP_InertialSensor LIBRARIES += AP_InertialNav -LIBRARIES += AP_NavEKF LIBRARIES += AP_NavEKF2 +LIBRARIES += AP_NavEKF3 LIBRARIES += AP_Mission LIBRARIES += AP_Rally LIBRARIES += AP_BattMonitor diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index ebd8adcd7a..de74f73a2a 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -34,8 +34,8 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ 'AP_InertialSensor', 'AP_Math', 'AP_Mission', - 'AP_NavEKF', 'AP_NavEKF2', + 'AP_NavEKF3', 'AP_Notify', 'AP_OpticalFlow', 'AP_Param',