CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Blimp/inertia.cpp
Views: 1798
1
#include "Blimp.h"

2


3
// read_inertia - read inertia in from accelerometers

4
void Blimp::read_inertia()

5
{

6
    // inertial altitude estimates. Use barometer climb rate during high vibrations

7
    inertial_nav.update(vibration_check.high_vibes);

8


9
    // pull position from ahrs

10
    Location loc;

11
    ahrs.get_location(loc);

12
    current_loc.lat = loc.lat;

13
    current_loc.lng = loc.lng;

14


15
    // exit immediately if we do not have an altitude estimate

16
    if (!inertial_nav.get_filter_status().flags.vert_pos) {

17
        return;

18
    }

19


20
    // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin

21
    const int32_t alt_above_origin_cm = inertial_nav.get_position_z_up_cm();

22
    current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);

23
    if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {

24
        // if home has not been set yet we treat alt-above-origin as alt-above-home

25
        current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME);

26
    }

27
}

28


29