Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Module: added velocity_ned to AHRS state

Browse Source
This commit is contained in:
Andrew Tridgell 2016-09-20 17:34:39 +10:00
parent 11c376588d
commit 95db9f61cc
2 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Module/AP_Module.cpp
View File

@ -201,6 +201,13 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS_NavEKF &ahrs)
state.gyro_bias[0] = gyro_bias[0];
state.gyro_bias[1] = gyro_bias[1];
state.gyro_bias[2] = gyro_bias[2];
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
state.velocity_ned[0] = vel.x;
state.velocity_ned[1] = vel.y;
state.velocity_ned[2] = vel.z;
}
for (const struct hook_list *h=hooks[HOOK_AHRS_UPDATE]; h; h=h->next) {
ap_hook_AHRS_update_fn_t fn = reinterpret_cast<ap_hook_AHRS_update_fn_t>(h->symbol);

5
libraries/AP_Module/AP_Module_Structures.h
View File

@ -15,7 +15,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#define AHRS_state_version 2
#define AHRS_state_version 3
#define gyro_sample_version 1
#define accel_sample_version 2
@ -92,6 +92,9 @@ struct AHRS_state {
// gyro_sample for primary_gyro. It should be added to a gyro
// sample to get the corrected gyro estimate
float gyro_bias[3];
// north-east-down velocity m/s
float velocity_ned[3];
};