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/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp
Views: 1800
1
//

2
// Simple test for the AP_AHRS interface

3
//

4


5
#include <AP_AHRS/AP_AHRS.h>

6
#include <AP_HAL/AP_HAL.h>

7
#include <AP_BoardConfig/AP_BoardConfig.h>

8
#include <GCS_MAVLink/GCS_Dummy.h>

9
#include <AP_RangeFinder/AP_RangeFinder.h>

10
#include <AP_Logger/AP_Logger.h>

11
#include <AP_GPS/AP_GPS.h>

12
#include <AP_Baro/AP_Baro.h>

13
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>

14
#include <AP_Vehicle/AP_Vehicle.h>

15


16
void setup();

17
void loop();

18


19
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

20


21


22
static AP_SerialManager serial_manager;

23


24
class DummyVehicle : public AP_Vehicle {

25
public:

26
    AP_AHRS ahrs{AP_AHRS::FLAG_ALWAYS_USE_EKF};

27
    bool set_mode(const uint8_t new_mode, const ModeReason reason) override { return true; };

28
    uint8_t get_mode() const override { return 1; };

29
    void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override {};

30
    void init_ardupilot() override {};

31
    void load_parameters() override {};

32
    void init() {

33
        BoardConfig.init();

34
        ins.init(100);

35
        ahrs.init();

36
    }

37
    AP_Int32 unused_log_bitmask;

38
    struct LogStructure log_structure[1] = {

39
    };

40
    const AP_Int32 &get_log_bitmask() override { return unused_log_bitmask; }

41


42
    const struct LogStructure *get_log_structures() const override {

43
        return log_structure;

44
    }

45
    uint8_t get_num_log_structures() const override {

46
        return 0;

47
    }

48
};

49


50
static DummyVehicle vehicle;

51


52
// choose which AHRS system to use

53
// AP_AHRS_DCM ahrs = AP_AHRS_DCM::create(barometer, gps);

54
auto &ahrs = vehicle.ahrs;

55


56
void setup(void)

57
{

58
    vehicle.init();

59
    serial_manager.init();

60
    AP::compass().init();

61
    if (!AP::compass().read()) {

62
        hal.console->printf("No compass detected\n");

63
    }

64
    AP::gps().init();

65
}

66


67
void loop(void)

68
{

69
    static uint16_t counter;

70
    static uint32_t last_t, last_print, last_compass;

71
    uint32_t now = AP_HAL::micros();

72
    float heading = 0;

73


74
    if (last_t == 0) {

75
        last_t = now;

76
        return;

77
    }

78
    last_t = now;

79


80
    if (now - last_compass > 100 * 1000UL &&

81
        AP::compass().read()) {

82
        heading = AP::compass().calculate_heading(ahrs.get_rotation_body_to_ned());

83
        // read compass at 10Hz

84
        last_compass = now;

85
    }

86


87
    ahrs.update();

88
    counter++;

89


90
    if (now - last_print >= 100000 /* 100ms : 10hz */) {

91
        Vector3f drift  = ahrs.get_gyro_drift();

92
        hal.console->printf(

93
                "r:%4.1f  p:%4.1f y:%4.1f "

94
                    "drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n",

95
                (double)ToDeg(ahrs.get_roll()),

96
                (double)ToDeg(ahrs.get_pitch()),

97
                (double)ToDeg(ahrs.get_yaw()),

98
                (double)ToDeg(drift.x),

99
                (double)ToDeg(drift.y),

100
                (double)ToDeg(drift.z),

101
                (double)(AP::compass().use_for_yaw() ? ToDeg(heading) : 0.0f),

102
                (double)((1.0e6f * counter) / (now-last_print)));

103
        last_print = now;

104
        counter = 0;

105
    }

106
}

107


108
const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {

109
    AP_GROUPEND

110
};

111
GCS_Dummy _gcs;

112


113
AP_HAL_MAIN();

114


115