ardupilot/libraries/SITL/SIM_InertialLabs.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

125 lines
3.8 KiB
C++
Raw Normal View History

2023-12-01 00:57:21 -04:00
/*
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/>.
*/
/*
simulate InertialLabs external AHRS
*/
#include "SIM_InertialLabs.h"
#include <GCS_MAVLink/GCS.h>
#include "SIM_GPS.h"
using namespace SITL;
InertialLabs::InertialLabs() : SerialDevice::SerialDevice()
{
}
void InertialLabs::send_packet(void)
{
const auto &fdm = _sitl->state;
pkt.msg_len = sizeof(pkt)-2;
pkt.accel_data_hr.x = (fdm.yAccel * 1.0e6)/GRAVITY_MSS;
pkt.accel_data_hr.y = (fdm.xAccel * 1.0e6)/GRAVITY_MSS;
pkt.accel_data_hr.z = (-fdm.zAccel * 1.0e6)/GRAVITY_MSS;
pkt.gyro_data_hr.y = fdm.rollRate*1.0e5;
pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5;
pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5;
float sigma, delta, theta;
AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta);
pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5;
pkt.baro_data.baro_alt = fdm.altitude;
pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1;
pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1;
pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS)*0.1;
pkt.orientation_angles.roll = fdm.rollDeg*100;
pkt.orientation_angles.pitch = fdm.pitchDeg*100;
pkt.orientation_angles.yaw = fdm.yawDeg*100;
pkt.velocity.x = fdm.speedE*100;
pkt.velocity.y = fdm.speedN*100;
pkt.velocity.z = -fdm.speedD*100;
pkt.position.lat = fdm.latitude*1e7;
pkt.position.lon = fdm.longitude*1e7;
pkt.position.alt = fdm.altitude*1e2;
pkt.kf_vel_covariance.x = 10;
pkt.kf_vel_covariance.z = 10;
pkt.kf_vel_covariance.z = 10;
pkt.kf_pos_covariance.x = 20;
pkt.kf_pos_covariance.z = 20;
pkt.kf_pos_covariance.z = 20;
const auto gps_tow = GPS_Backend::gps_time();
pkt.gps_ins_time_ms = gps_tow.ms;
pkt.gnss_new_data = 0;
if (packets_sent % gps_frequency == 0) {
// update GPS data at 5Hz
pkt.gps_week = gps_tow.week;
pkt.gnss_pos_timestamp = gps_tow.ms;
pkt.gnss_new_data = 3;
pkt.gps_position.lat = pkt.position.lat;
pkt.gps_position.lon = pkt.position.lon;
pkt.gps_position.alt = pkt.position.alt;
pkt.num_sats = 32;
pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE)*100;
Vector2d track{fdm.speedN,fdm.speedE};
pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle()))*100;
pkt.gnss_vel_track.ver_speed = -fdm.speedD*100;
pkt.gnss_extended_info.fix_type = 2;
}
2024-02-19 14:43:24 -04:00
pkt.differential_pressure = 0.5*sq(fdm.airspeed+fabsF(rand_float()*0.25))*1.0e4;
2023-12-01 00:57:21 -04:00
pkt.supply_voltage = 12.3*100;
pkt.temperature = 23.4*10;
const uint8_t *buffer = (const uint8_t *)&pkt;
pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4);
write_to_autopilot((char *)&pkt, sizeof(pkt));
packets_sent++;
}
/*
send InertialLabs data
*/
void InertialLabs::update(void)
{
if (!init_sitl_pointer()) {
return;
}
const uint32_t us_between_packets = 5000; // 200Hz
const uint32_t now = AP_HAL::micros();
if (now - last_pkt_us >= us_between_packets) {
last_pkt_us = now;
send_packet();
}
}