zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tools: Add EKF3 and remove EK1

master
Andrew Tridgell 9 years ago
parent
2facebff80
commit
35bed59bd4
7 changed files with 28 additions and 242 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      Tools/CPUInfo/make.inc
  2. 3
      Tools/Hello/make.inc
  3. 4
      Tools/Replay/Parameters.h
  4. 250
      Tools/Replay/Replay.cpp
  5. 6
      Tools/Replay/Replay.h
  6. 2
      Tools/Replay/make.inc
  7. 2
      Tools/ardupilotwaf/ardupilotwaf.py

3
Tools/CPUInfo/make.inc
Unescape View File

@ -10,7 +10,8 @@ LIBRARIES += AP_GPS @@ -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

3
Tools/Hello/make.inc
Unescape View File

@ -14,7 +14,8 @@ LIBRARIES += AP_Vehicle @@ -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

4
Tools/Replay/Parameters.h
Unescape View File

@ -12,10 +12,10 @@ public: @@ -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;
};

250
Tools/Replay/Replay.cpp
Unescape View File

@ -64,10 +64,6 @@ const AP_Param::Info ReplayVehicle::var_info[] = { @@ -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[] = { @@ -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) @@ -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) @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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);
}

6
Tools/Replay/Replay.h
Unescape View File

@ -36,8 +36,8 @@ @@ -36,8 +36,8 @@
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_InertialNav/AP_InertialNav.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
@ -63,9 +63,9 @@ public: @@ -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;

2
Tools/Replay/make.inc
Unescape View File

@ -18,8 +18,8 @@ LIBRARIES += AP_Compass @@ -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

2
Tools/ardupilotwaf/ardupilotwaf.py
Unescape View File

@ -34,8 +34,8 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ @@ -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',

Write Preview
Loading…
Cancel
Save