fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move geo and geo_lookup from PX4 Firmware to ECL

Browse Source
This commit is contained in:
Daniel Agar
2018-03-26 21:52:11 -04:00
parent 1bb4c17c0e
commit cd12f049fe
25 changed files with 392 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+5 -4
View File
@@ -39,9 +39,10 @@
* @author Paul Riseborough <p_riseborough@live.com.au>
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
bool Ekf::init(uint64_t timestamp)
{
@@ -359,7 +360,7 @@ void Ekf::predictState()
_state.vel += corrected_delta_vel_ef;
// compensate for acceleration due to gravity
_state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt;
_state.vel(2) += CONSTANTS_ONE_G * _imu_sample_delayed.delta_vel_dt;
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
@@ -485,7 +486,7 @@ void Ekf::calculateOutputStates()
Vector3f delta_vel_NED = _R_to_earth_now * delta_vel;
// corrrect for measured accceleration due to gravity
delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt;
delta_vel_NED(2) += CONSTANTS_ONE_G * imu_new.delta_vel_dt;
// calculate the earth frame velocity derivatives
if (imu_new.delta_vel_dt > 1e-4f) {