5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00

AP_AHRS: example fix travis warning

missing function declaration
implicit cast
some style fix
This commit is contained in:
Pierre Kancir 2017-04-13 13:29:38 +02:00 committed by Francisco Ferreira
parent 320c5e1b96
commit fede727d38

View File

@ -8,6 +8,9 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <GCS_MAVLink/GCS.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// INS and Baro declaration
@ -34,10 +37,6 @@ static DummyVehicle vehicle;
// AP_AHRS_DCM ahrs(ins, baro, gps);
AP_AHRS_NavEKF ahrs(vehicle.ahrs);
#define HIGH 1
#define LOW 0
void setup(void)
{
AP_BoardConfig{}.init();
@ -83,14 +82,14 @@ void loop(void)
hal.console->printf(
"r:%4.1f p:%4.1f y:%4.1f "
"drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n",
ToDeg(ahrs.roll),
ToDeg(ahrs.pitch),
ToDeg(ahrs.yaw),
ToDeg(drift.x),
ToDeg(drift.y),
ToDeg(drift.z),
compass.use_for_yaw() ? ToDeg(heading) : 0.0f,
(1.0e6f*counter)/(now-last_print));
(double)ToDeg(ahrs.roll),
(double)ToDeg(ahrs.pitch),
(double)ToDeg(ahrs.yaw),
(double)ToDeg(drift.x),
(double)ToDeg(drift.y),
(double)ToDeg(drift.z),
(double)(compass.use_for_yaw() ? ToDeg(heading) : 0.0f),
(double)((1.0e6f * counter) / (now-last_print)));
last_print = now;
counter = 0;
}