aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use AP_AHRS_NavEKF when available

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-02 12:16:20 +11:00
parent 7ac78ff991
commit 9ac886f58d
4 changed files with 41 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/ArduPlane.pde
View File

@ -273,7 +273,12 @@ AP_InertialSensor_L3G4200D ins;
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, g_gps, barometer);
#else
AP_AHRS_DCM ahrs(ins, g_gps);
#endif
static AP_L1_Control L1_controller(ahrs);
static AP_TECS TECS_controller(ahrs, aparm);
@ -284,11 +289,6 @@ static AP_PitchController pitchController(ahrs, aparm);
static AP_YawController yawController(ahrs, aparm);
static AP_SteerController steerController(ahrs);
// Inertial Navigation EKF
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
static NavEKF NavEKF(ahrs, barometer);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
#endif
@ -807,14 +807,10 @@ static void ahrs_update()
#endif
ahrs.update();
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
NavEKF.UpdateFilter();
#endif
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_EKF1();
Log_Write_EKF2();
Log_Write_EKF();
}
if (should_log(MASK_LOG_IMU))
@ -898,8 +894,7 @@ static void update_logging1(void)
{
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_EKF1();
Log_Write_EKF2();
Log_Write_EKF();
}
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))

6
ArduPlane/GCS_Mavlink.pde
View File

@ -530,11 +530,11 @@ static void NOINLINE send_simstate(mavlink_channel_t chan)
// report NavEKF state
static void NOINLINE send_ekf(mavlink_channel_t chan)
{
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#if AP_AHRS_NAVEKF_AVAILABLE
Vector3f euler;
struct Location loc;
NavEKF.getEulerAngles(euler);
NavEKF.getLLH(loc);
ahrs.get_NavEKF().getEulerAngles(euler);
ahrs.get_NavEKF().getLLH(loc);
mavlink_msg_ekf_send(chan,
euler.x,
euler.y,

68
ArduPlane/Log.pde
View File

@ -206,20 +206,37 @@ struct PACKED log_EKF1 {
int8_t gyrZ;
};
struct PACKED log_EKF2 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int8_t accX;
int8_t accY;
int8_t accZ;
int16_t windN;
int16_t windE;
int16_t magN;
int16_t magE;
int16_t magD;
int16_t magX;
int16_t magY;
int16_t magZ;
};
// Write first EKF packet
static void Log_Write_EKF1(void)
static void Log_Write_EKF(void)
{
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#if AP_AHRS_NAVEKF_AVAILABLE
Vector3f euler;
Vector3f posNED;
Vector3f velNED;
Vector3f dAngBias;
Vector3f dVelBias;
Vector3f gyroBias;
NavEKF.getEulerAngles(euler);
NavEKF.getVelNED(velNED);
NavEKF.getPosNED(posNED);
NavEKF.getGyroBias(gyroBias);
ahrs.get_NavEKF().getEulerAngles(euler);
ahrs.get_NavEKF().getVelNED(velNED);
ahrs.get_NavEKF().getPosNED(posNED);
ahrs.get_NavEKF().getGyroBias(gyroBias);
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG),
time_ms : hal.scheduler->millis(),
@ -237,39 +254,16 @@ static void Log_Write_EKF1(void)
gyrZ : (int8_t)(gyroBias.z) // deg/min
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
}
struct PACKED log_EKF2 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int8_t accX;
int8_t accY;
int8_t accZ;
int16_t windN;
int16_t windE;
int16_t magN;
int16_t magE;
int16_t magD;
int16_t magX;
int16_t magY;
int16_t magZ;
};
// Write second EKF packet
static void Log_Write_EKF2(void)
{
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
Vector3f euler;
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
NavEKF.getAccelBias(accelBias);
NavEKF.getWind(wind);
NavEKF.getMagNED(magNED);
NavEKF.getMagXYZ(magXYZ);
struct log_EKF2 pkt = {
ahrs.get_NavEKF().getAccelBias(accelBias);
ahrs.get_NavEKF().getWind(wind);
ahrs.get_NavEKF().getMagNED(magNED);
ahrs.get_NavEKF().getMagXYZ(magXYZ);
struct log_EKF2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG),
time_ms : hal.scheduler->millis(),
accX : (int8_t)(100*accelBias.x),
@ -284,10 +278,11 @@ static void Log_Write_EKF2(void)
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
#endif
}
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint32_t loop_time;
@ -726,8 +721,7 @@ static void Log_Write_Nav_Tuning() {}
static void Log_Write_TECS_Tuning() {}
static void Log_Write_Performance() {}
static void Log_Write_Attitude() {}
static void Log_Write_EKF1() {}
static void Log_Write_EKF2() {}
static void Log_Write_EKF() {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Camera() {}
static void Log_Write_Mode(uint8_t mode) {}

5
ArduPlane/commands.pde
View File

@ -239,11 +239,6 @@ static void init_home()
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude_cm;
// startup EKF now we have home
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
NavEKF.InitialiseFilter();
#endif
}
/*