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

NavEKF: support sending EKF_STATUS_REPORT

Browse Source
This commit is contained in:
Randy Mackay 2015-03-10 16:00:32 +09:00
parent 5ee67e63ec
commit 230ca583d1
2 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4569,6 +4569,37 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const
status.flags.pred_horiz_pos_abs = gpsNavPossible; // we should be able to estimate an absolute position when we enter flight mode
}
// send an EKF_STATUS message to GCS
void NavEKF::send_status_report(mavlink_channel_t chan)
{
// get filter status
nav_filter_status filt_state;
getFilterStatus(filt_state);
// prepare flags
uint16_t flags = 0;
if (filt_state.flags.attitude) { flags |= EKF_ATTITUDE; }
if (filt_state.flags.horiz_vel) { flags |= EKF_VELOCITY_HORIZ; }
if (filt_state.flags.vert_vel) { flags |= EKF_VELOCITY_VERT; }
if (filt_state.flags.horiz_pos_rel) { flags |= EKF_POS_HORIZ_REL; }
if (filt_state.flags.horiz_pos_abs) { flags |= EKF_POS_HORIZ_ABS; }
if (filt_state.flags.vert_pos) { flags |= EKF_POS_VERT_ABS; }
if (filt_state.flags.terrain_alt) { flags |= EKF_POS_VERT_AGL; }
if (filt_state.flags.const_pos_mode) { flags |= EKF_CONST_POS_MODE; }
if (filt_state.flags.pred_horiz_pos_rel) { flags |= EKF_PRED_POS_HORIZ_REL; }
if (filt_state.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; }
// get variances
float velVar, posVar, hgtVar, tasVar;
Vector3f magVar;
Vector2f offset;
getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
// send message
mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), tasVar);
}
// Check arm status and perform required checks and mode changes
void NavEKF::performArmingChecks()
{

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -28,6 +28,7 @@
#include <AP_Compass.h>
#include <AP_Param.h>
#include <AP_Nav_Common.h>
#include <GCS_MAVLink.h>
// #define MATH_CHECK_INDEXES 1
@ -224,6 +225,9 @@ public:
*/
void getFilterStatus(nav_filter_status &status) const;
// send an EKF_STATUS_REPORT message to GCS
void send_status_report(mavlink_channel_t chan);
static const struct AP_Param::GroupInfo var_info[];
private: