mirror of https://github.com/ArduPilot/ardupilot
149 lines
3.8 KiB
C++
149 lines
3.8 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
simple vicon simulator class
|
|
|
|
XKFR
|
|
|
|
*/
|
|
|
|
#include "SIM_Vicon.h"
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
#include <fcntl.h>
|
|
|
|
using namespace SITL;
|
|
|
|
Vicon::Vicon()
|
|
{
|
|
int tmp[2];
|
|
if (pipe(tmp) == -1) {
|
|
AP_HAL::panic("pipe() failed");
|
|
}
|
|
fd_my_end = tmp[1];
|
|
fd_their_end = tmp[0];
|
|
|
|
// close file descriptors on exec:
|
|
fcntl(fd_my_end, F_SETFD, FD_CLOEXEC);
|
|
fcntl(fd_their_end, F_SETFD, FD_CLOEXEC);
|
|
|
|
// 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()
|
|
{
|
|
const uint32_t now = AP_HAL::millis();
|
|
|
|
if (now - last_heartbeat_ms < 100) {
|
|
// we only provide a heartbeat every so often
|
|
return;
|
|
}
|
|
last_heartbeat_ms = now;
|
|
|
|
mavlink_message_t msg;
|
|
mavlink_msg_heartbeat_pack(system_id,
|
|
component_id,
|
|
&msg,
|
|
MAV_TYPE_GCS,
|
|
MAV_AUTOPILOT_INVALID,
|
|
0,
|
|
0,
|
|
0);
|
|
}
|
|
|
|
void Vicon::update_vicon_position_estimate(const Location &loc,
|
|
const Vector3f &position,
|
|
const Quaternion &attitude)
|
|
{
|
|
const uint64_t now_us = AP_HAL::micros64();
|
|
|
|
if (time_offset_us == 0) {
|
|
time_offset_us = (unsigned(random()) % 7000) * 1000000ULL;
|
|
printf("time_offset_us %llu\n", (long long unsigned)time_offset_us);
|
|
}
|
|
|
|
if (time_send_us && now_us >= time_send_us) {
|
|
uint8_t msgbuf[300];
|
|
uint16_t msgbuf_len = mavlink_msg_to_send_buffer(msgbuf, &obs_msg);
|
|
|
|
if (::write(fd_my_end, (void*)&msgbuf, msgbuf_len) != msgbuf_len) {
|
|
::fprintf(stderr, "Vicon: write failure\n");
|
|
}
|
|
time_send_us = 0;
|
|
}
|
|
if (time_send_us != 0) {
|
|
// waiting for the last msg to go out
|
|
return;
|
|
}
|
|
|
|
if (now_us - last_observation_usec < 10000) {
|
|
// create observations at 10ms
|
|
return;
|
|
}
|
|
|
|
float roll;
|
|
float pitch;
|
|
float yaw;
|
|
attitude.to_euler(roll, pitch, yaw);
|
|
|
|
mavlink_msg_vicon_position_estimate_pack_chan(
|
|
system_id,
|
|
component_id,
|
|
mavlink_ch,
|
|
&obs_msg,
|
|
now_us + time_offset_us,
|
|
position.x,
|
|
position.y,
|
|
position.z,
|
|
roll,
|
|
pitch,
|
|
yaw,
|
|
NULL);
|
|
|
|
uint32_t delay_ms = 25 + unsigned(random()) % 300;
|
|
time_send_us = now_us + delay_ms * 1000UL;
|
|
}
|
|
|
|
bool Vicon::init_sitl_pointer()
|
|
{
|
|
if (_sitl == nullptr) {
|
|
_sitl = AP::sitl();
|
|
if (_sitl == nullptr) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
update vicon sensor state
|
|
*/
|
|
void Vicon::update(const Location &loc, const Vector3f &position, const Quaternion &attitude)
|
|
{
|
|
if (!init_sitl_pointer()) {
|
|
return;
|
|
}
|
|
|
|
maybe_send_heartbeat();
|
|
update_vicon_position_estimate(loc, position, attitude);
|
|
}
|
|
|