aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: send_home and send_home_all methods

Browse Source
This commit is contained in:
Randy Mackay 2015-10-02 17:55:57 +09:00
parent 88f32bc86f
commit 9b96a2c385
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -153,6 +153,8 @@ public:
void send_autopilot_version(uint8_t major_version, uint8_t minor_version, uint8_t patch_version, uint8_t version_type) const;
void send_local_position(const AP_AHRS &ahrs) const;
void send_vibration(const AP_InertialSensor &ins) const;
void send_home(const Location &home) const;
static void send_home_all(const Location &home);
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels

35
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1384,3 +1384,38 @@ void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const
ins.get_accel_clip_count(2));
#endif
}
void GCS_MAVLINK::send_home(const Location &home) const
{
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
mavlink_msg_home_position_send(
chan,
home.lat,
home.lng,
home.alt / 100,
0.0f, 0.0f, 0.0f,
q,
0.0f, 0.0f, 0.0f);
}
}
void GCS_MAVLINK::send_home_all(const Location &home)
{
const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_HOME_POSITION_LEN) {
mavlink_msg_home_position_send(
chan,
home.lat,
home.lng,
home.alt / 100,
0.0f, 0.0f, 0.0f,
q,
0.0f, 0.0f, 0.0f);
}
}
}
}