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

SITL: SIM_Vicon: use existing channel for packing messages

Browse Source 
Also correct sending of Vicon message; too many bytes were sent
This commit is contained in:
Peter Barker 2018-03-31 20:05:23 +11:00 committed by Randy Mackay
parent 42547d2d6d
commit 871661c65f
2 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/SITL/SIM_Vicon.cpp
View File

@ -38,6 +38,10 @@ Vicon::Vicon()
// make sure we don't screw the simulation up by blocking:
fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK);
fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK);
if (!valid_channel(mavlink_ch)) {
AP_HAL::panic("Invalid mavlink channel");
}
}
void Vicon::maybe_send_heartbeat()
@ -100,20 +104,21 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
observation.attitude.to_euler(roll, pitch, yaw);
mavlink_message_t msg;
mavlink_msg_vicon_position_estimate_pack_chan(system_id,
component_id,
mavlink_ch,
&msg,
observation.time_ms*1000,
observation.position.x,
observation.position.y,
observation.position.z,
roll,
pitch,
yaw);
const uint16_t n = mavlink_msg_vicon_position_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg,
observation.time_ms*1000,
observation.position.x,
observation.position.y,
observation.position.z,
roll,
pitch,
yaw);
// ::fprintf(stderr, "Vicon: writing pos=(%03.03f %03.03f %03.03f) att=(%01.03f %01.03f %01.03f)\n", observation.position.x, observation.position.y, observation.position.z, roll, pitch, yaw);
if (::write(fd_my_end, (void*)&msg, sizeof(msg)) != sizeof(msg)) {
// ::fprintf(stderr, "Vicon: %u: writing pos=(%03.03f %03.03f %03.03f) att=(%01.03f %01.03f %01.03f)\n", observation.time_ms, observation.position.x, observation.position.y, observation.position.z, roll, pitch, yaw);
if (::write(fd_my_end, (void*)&msg, n) != n) {
::fprintf(stderr, "Vicon: write failure\n");
// abort();
}

2
libraries/SITL/SIM_Vicon.h
View File

@ -47,7 +47,7 @@ private:
const uint8_t component_id = 18;
// we share channels with the ArduPilot binary!
const uint8_t mavlink_ch = 17;
const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
int fd_their_end;
int fd_my_end;