Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: enable NavEKF logging in plane

This commit is contained in:
Andrew Tridgell 2013-12-30 16:19:27 +11:00
parent b4153c4d32
commit 1fbae4dd44
3 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.pde
View File

@ -805,6 +805,7 @@ static void ahrs_update()
#endif #endif
ahrs.update(); ahrs.update();
NavEKF.UpdateFilter();
if (should_log(MASK_LOG_ATTITUDE_FAST)) if (should_log(MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude(); Log_Write_Attitude();

18
ArduPlane/GCS_Mavlink.pde
View File

@ -527,6 +527,22 @@ static void NOINLINE send_simstate(mavlink_channel_t chan)
#endif #endif
} }
// report NavEKF state
static void NOINLINE send_ekf(mavlink_channel_t chan)
{
Vector3f euler;
struct Location loc;
NavEKF.getEulerAngles(euler);
NavEKF.getLLH(loc);
mavlink_msg_ekf_send(chan,
euler.x,
euler.y,
euler.z,
loc.alt*1.0e-2f,
loc.lat,
loc.lng);
}
static void NOINLINE send_hwstatus(mavlink_channel_t chan) static void NOINLINE send_hwstatus(mavlink_channel_t chan)
{ {
mavlink_msg_hwstatus_send( mavlink_msg_hwstatus_send(
@ -726,6 +742,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id)
case MSG_SIMSTATE: case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE); CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate(chan); send_simstate(chan);
CHECK_PAYLOAD_SIZE(EKF);
send_ekf(chan);
break; break;
case MSG_HWSTATUS: case MSG_HWSTATUS:

2
ArduPlane/commands.pde
View File

@ -240,6 +240,8 @@ static void init_home()
guided_WP = home; guided_WP = home;
guided_WP.alt += g.RTL_altitude_cm; guided_WP.alt += g.RTL_altitude_cm;
// startup EKF now we have home
NavEKF.InitialiseFilter();
} }
/* /*