Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simulate InertialLabs external AHRS
      17             : */
      18             : #include "SIM_InertialLabs.h"
      19             : #include <GCS_MAVLink/GCS.h>
      20             : #include "SIM_GPS.h"
      21             : 
      22             : using namespace SITL;
      23             : 
      24          17 : InertialLabs::InertialLabs() : SerialDevice::SerialDevice()
      25             : {
      26          17 : }
      27             : 
      28       39575 : void InertialLabs::send_packet(void)
      29             : {
      30       39575 :     const auto &fdm = _sitl->state;
      31             : 
      32       39575 :     pkt.msg_len = sizeof(pkt)-2;
      33             : 
      34       39575 :     const auto gps_tow = GPS_Backend::gps_time();
      35             : 
      36             :     // 0x01 GPS INS Time (round)
      37       39575 :     pkt.gnss_ins_time_ms = gps_tow.ms;
      38             : 
      39             :     // 0x23 Accelerometer data HR
      40       39575 :     pkt.accel_data_hr.x = (fdm.yAccel / GRAVITY_MSS) * 1.0e6;  // g*1.0e6
      41       39575 :     pkt.accel_data_hr.y = (fdm.xAccel / GRAVITY_MSS) * 1.0e6;  // g*1.0e6
      42       39575 :     pkt.accel_data_hr.z = (-fdm.zAccel / GRAVITY_MSS) * 1.0e6; // g*1.0e6
      43             : 
      44             :     // 0x21 Gyro data HR
      45       39575 :     pkt.gyro_data_hr.y = fdm.rollRate * 1.0e5;  // deg/s*1.0e5
      46       39575 :     pkt.gyro_data_hr.x = fdm.pitchRate * 1.0e5; // deg/s*1.0e5
      47       39575 :     pkt.gyro_data_hr.z = -fdm.yawRate * 1.0e5;  // deg/s*1.0e5
      48             : 
      49             :     // 0x25 Barometer data
      50             :     float p, t_K;
      51       39575 :     AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude+rand_float()*0.25, p, t_K);
      52             : 
      53       39575 :     pkt.baro_data.pressure_pa2 = p * 0.5; // Pa/2
      54       39575 :     pkt.baro_data.baro_alt = fdm.altitude * 100; // m
      55             : 
      56             :     // 0x24 Magnetometer data
      57       39575 :     pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS) * 0.1;  // nT/10
      58       39575 :     pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS) * 0.1;  // nT/10
      59       39575 :     pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS) * 0.1; // nT/10
      60             : 
      61             :     // 0x07 Orientation angles
      62             :     float roll_rad, pitch_rad, yaw_rad, heading_deg;
      63       39575 :     fdm.quaternion.to_euler(roll_rad, pitch_rad, yaw_rad);
      64       39575 :     heading_deg = fmodf(degrees(yaw_rad), 360.0f);
      65       39575 :     if (heading_deg < 0.0f) {
      66       35153 :         heading_deg += 360.0f;
      67             :     }
      68       39575 :     pkt.orientation_angles.roll = roll_rad * RAD_TO_DEG * 100; // deg*100
      69       39575 :     pkt.orientation_angles.pitch = pitch_rad * RAD_TO_DEG * 100; // deg*100
      70       39575 :     pkt.orientation_angles.yaw = heading_deg * 100; // deg*100
      71             : 
      72             :     // 0x12 Velocities
      73       39575 :     pkt.velocity.x = fdm.speedE * 100;  // m/s*100
      74       39575 :     pkt.velocity.y = fdm.speedN * 100;  // m/s*100
      75       39575 :     pkt.velocity.z = -fdm.speedD * 100; // m/s*100
      76             : 
      77             :     // 0x10 Position
      78       39575 :     pkt.position.lat = fdm.latitude * 1e7;  // deg*1.0e7
      79       39575 :     pkt.position.lon = fdm.longitude * 1e7; // deg*1.0e7
      80       39575 :     pkt.position.alt = fdm.altitude * 1e2;  // m*100
      81             : 
      82             :     // 0x58 KF velocity covariance
      83       39575 :     pkt.kf_vel_covariance.x = 10; // mm/s
      84       39575 :     pkt.kf_vel_covariance.y = 10; // mm/s
      85       39575 :     pkt.kf_vel_covariance.z = 10; // mm/s
      86             : 
      87             :     // 0x57 KF position covariance HR
      88       39575 :     pkt.kf_pos_covariance.x = 20; // mm
      89       39575 :     pkt.kf_pos_covariance.y = 20; // mm
      90       39575 :     pkt.kf_pos_covariance.z = 20; // mm
      91             : 
      92             :     // 0x53 Unit status word (USW)
      93       39575 :     pkt.unit_status = 0; // INS data is valid
      94             : 
      95             :     // 0x28 Differential pressure
      96       39575 :     pkt.differential_pressure = fdm.airspeed_raw_pressure[0] * 0.01 * 1.0e4; // mbar*1.0e4 (0.01 - Pa to mbar)
      97             : 
      98             :     // 0x86 True airspeed (TAS)
      99       39575 :     pkt.true_airspeed = fdm.airspeed * 100; // m/s*100
     100             : 
     101             :     // 0x8A Wind speed
     102       39575 :     pkt.wind_speed.x = fdm.wind_ef.y * 100;
     103       39575 :     pkt.wind_speed.y = fdm.wind_ef.x * 100;
     104       39575 :     pkt.wind_speed.z = 0;
     105             : 
     106             :     // 0x8D ADU status
     107       39575 :     pkt.air_data_status = 0; // ADU data is valid
     108             : 
     109             :     // 0x50 Supply voltage
     110       39575 :     pkt.supply_voltage = 12.3 * 100; // VDC*100
     111             : 
     112             :     // 0x52 Temperature
     113       39575 :     pkt.temperature = KELVIN_TO_C(t_K)*10; // degC
     114             : 
     115             :     // 0x5A Unit status word (USW2)
     116       39575 :     pkt.unit_status2 = 0; // INS data is valid
     117             : 
     118             :     // 0x54 INS (Navigation) solution status
     119       39575 :     pkt.ins_sol_status = 0; // INS solution is good
     120             : 
     121       39575 :     pkt.gnss_new_data = 0;
     122       39575 :     if (packets_sent % gnss_frequency == 0) {
     123             :         // 0x3C GPS week
     124        1989 :         pkt.gnss_week = gps_tow.week;
     125             : 
     126             :         // 0x4A GNSS extended info
     127        1989 :         pkt.gnss_extended_info.fix_type = 2; // 3D fix
     128        1989 :         pkt.gnss_extended_info.spoofing_status = 1; // no spoofing indicated
     129             : 
     130             :         // 0x3B Number of satellites used in solution
     131        1989 :         pkt.num_sats = 32;
     132             : 
     133             :         // 0x30 GNSS Position
     134        1989 :         pkt.gnss_position.lat = fdm.latitude * 1e7;  // deg*1.0e7
     135        1989 :         pkt.gnss_position.lon = fdm.longitude * 1e7; // deg*1.0e7
     136        1989 :         pkt.gnss_position.alt = fdm.altitude * 1e2;  // m*100
     137             : 
     138             :         // 0x32 GNSS Velocity, Track over ground
     139        1989 :         Vector2d track{fdm.speedN,fdm.speedE};
     140        1989 :         pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE) * 100; // m/s*100
     141        1989 :         pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle())) * 100; // deg*100
     142        1989 :         pkt.gnss_vel_track.ver_speed = -fdm.speedD * 100; // m/s*100
     143             : 
     144             :         // 0x3E GNSS Position timestamp
     145        1989 :         pkt.gnss_pos_timestamp = gps_tow.ms;
     146             : 
     147             :         // 0x36 GNSS info short
     148        1989 :         pkt.gnss_info_short.info1 = 0; // 0 – Single point position
     149        1989 :         pkt.gnss_info_short.info2 = 12; // bit 2 and 3 are set (Time is fine set and is being steered)
     150             : 
     151             :         // 0x41 New GPS
     152        1989 :         pkt.gnss_new_data = 3; // GNSS position and velocity data update
     153             : 
     154             :         // 0xС0 u-blox jamming status
     155        1989 :         pkt.gnss_jam_status = 1; // ok (no significant jamming)
     156             : 
     157             :         // 0x33 GNSS Heading, GNSS Pitch
     158        1989 :         pkt.gnss_angles.heading = 0; // only for dual-antenna GNSS receiver
     159        1989 :         pkt.gnss_angles.pitch = 0; // only for dual-antenna GNSS receiver
     160             : 
     161             :         // 0x3A GNSS Angles position type
     162        1989 :         pkt.gnss_angle_pos_type = 0; // only for dual-antenna GNSS receiver
     163             : 
     164             :         // 0x40 GNSS Heading timestamp
     165        1989 :         pkt.gnss_heading_timestamp = 0; // only for dual-antenna GNSS receiver
     166             : 
     167             :         // 0x42 Dilution of precision
     168        1989 :         pkt.gnss_dop.gdop = 1000; // *1.0e3
     169        1989 :         pkt.gnss_dop.pdop = 1000; // *1.0e3
     170        1989 :         pkt.gnss_dop.hdop = 1000; // *1.0e3
     171        1989 :         pkt.gnss_dop.vdop = 1000; // *1.0e3
     172        1989 :         pkt.gnss_dop.tdop = 1000; // *1.0e3
     173             :     }
     174             : 
     175       39575 :     const uint8_t *buffer = (const uint8_t *)&pkt;
     176       39575 :     pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4);
     177             : 
     178       39575 :     write_to_autopilot((char *)&pkt, sizeof(pkt));
     179             : 
     180       39575 :     packets_sent++;
     181       39575 : }
     182             : 
     183             : 
     184             : /*
     185             :   send InertialLabs data
     186             :  */
     187      277079 : void InertialLabs::update(void)
     188             : {
     189      277079 :     if (!init_sitl_pointer()) {
     190           0 :         return;
     191             :     }
     192      277079 :     const uint32_t us_between_packets = 5000; // 200Hz
     193      277079 :     const uint32_t now = AP_HAL::micros();
     194      277079 :     if (now - last_pkt_us >= us_between_packets) {
     195       39575 :         last_pkt_us = now;
     196       39575 :         send_packet();
     197             :     }
     198             : 
     199             : }