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

GCS_MAVLink: moved send_ahrs2 to common code

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-04 11:30:32 +11:00
parent 66dbaa6657
commit 9aea781248
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -181,6 +181,7 @@ public:
// common send functions
void send_meminfo(void);
void send_power_status(void);
void send_ahrs2(AP_AHRS &ahrs);
private:
void handleMessage(mavlink_message_t * msg);

19
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -18,6 +18,7 @@
*/
#include <GCS.h>
#include <AP_AHRS.h>
extern const AP_HAL::HAL& hal;
@ -159,3 +160,21 @@ void GCS_MAVLINK::send_power_status(void)
hal.analogin->power_status_flags());
#endif
}
// report AHRS2 state
void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
{
#if AP_AHRS_NAVEKF_AVAILABLE
Vector3f euler;
struct Location loc;
if (ahrs.get_secondary_attitude(euler) && ahrs.get_secondary_position(loc)) {
mavlink_msg_ahrs2_send(chan,
euler.x,
euler.y,
euler.z,
loc.alt*1.0e-2f,
loc.lat,
loc.lng);
}
#endif
}