From efb74b0f9e3eeabadc32188ee79c342523d3736a Mon Sep 17 00:00:00 2001 From: BLash Date: Thu, 18 Jul 2024 19:27:08 -0500 Subject: [PATCH] AP_ExternalAHRS_VectorNav: Update SIM to match new message definitions Redefine messages to 3 INS packets, and send as appropriate --- libraries/SITL/SIM_VectorNav.cpp | 207 ++++++++++++++++++++----------- libraries/SITL/SIM_VectorNav.h | 11 +- 2 files changed, 139 insertions(+), 79 deletions(-) diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index ed4c1bcaedd53d..44499377d48b8e 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -29,37 +29,49 @@ VectorNav::VectorNav() : { } -struct PACKED VN_INS_packet1 { - float uncompMag[3]; + +struct PACKED VN_IMU_packet_sim { + static constexpr uint8_t header[]{0x01, 0x21, 0x07}; + uint64_t timeStartup; + float gyro[3]; + float accel[3]; float uncompAccel[3]; float uncompAngRate[3]; - float pressure; float mag[3]; - float accel[3]; - float gyro[3]; + float temp; + float pressure; +}; +constexpr uint8_t VN_IMU_packet_sim::header[]; + +struct PACKED VN_INS_ekf_packet_sim { + static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06}; + uint64_t timeStartup; float ypr[3]; float quaternion[4]; float yprU[3]; - double positionLLA[3]; - float velNED[3]; + uint16_t insStatus; + double posLla[3]; + float velNed[3]; float posU; float velU; }; - -struct PACKED VN_INS_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; +constexpr uint8_t VN_INS_ekf_packet_sim::header[]; + +struct PACKED VN_INS_gnss_packet_sim { + static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00}; + uint64_t timeStartup; + uint64_t timeGps; + uint8_t numSats1; + uint8_t fix1; + double posLla1[3]; + float velNed1[3]; + float posU1[3]; + float velU1; + float dop1[7]; + uint8_t numSats2; + uint8_t fix2; }; - -#define VN_PKT1_HEADER { 0xFA, 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 } -#define VN_PKT2_HEADER { 0xFA, 0x4E, 0x02, 0x00, 0x10, 0x00, 0xB8, 0x20, 0x18, 0x00 } +constexpr uint8_t VN_INS_gnss_packet_sim::header[]; /* get timeval using simulation time @@ -80,36 +92,62 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -void VectorNav::send_packet1(void) +void VectorNav::send_imu_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet1 pkt {}; + struct VN_IMU_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; + + + const float gyro_noise = 0.05; + + pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); + pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); + pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + + pkt.accel[0] = fdm.xAccel; + pkt.accel[1] = fdm.yAccel; + pkt.accel[2] = fdm.zAccel; pkt.uncompAccel[0] = fdm.xAccel; pkt.uncompAccel[1] = fdm.yAccel; pkt.uncompAccel[2] = fdm.zAccel; - const float gyro_noise = 0.05; + pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float()); pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float()); pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float()); - const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); - pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; - pkt.mag[0] = fdm.bodyMagField.x*0.001; pkt.mag[1] = fdm.bodyMagField.y*0.001; pkt.mag[2] = fdm.bodyMagField.z*0.001; - pkt.uncompMag[0] = pkt.mag[0]; - pkt.uncompMag[1] = pkt.mag[1]; - pkt.uncompMag[2] = pkt.mag[2]; - pkt.accel[0] = fdm.xAccel; - pkt.accel[1] = fdm.yAccel; - pkt.accel[2] = fdm.zAccel; - pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); - pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); - pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); + + const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)sync_byte, 1); + write_to_autopilot((char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header)); + write_to_autopilot((char *)&pkt, sizeof(pkt)); + + uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0); + crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; + swab(&crc, &crc2, 2); + + write_to_autopilot((char *)&crc2, sizeof(crc2)); +} + +void VectorNav::send_ins_ekf_packet(void) +{ + const auto &fdm = _sitl->state; + + struct VN_INS_ekf_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; pkt.ypr[0] = fdm.yawDeg; pkt.ypr[1] = fdm.pitchDeg; @@ -120,58 +158,74 @@ void VectorNav::send_packet1(void) pkt.quaternion[2] = fdm.quaternion.q4; pkt.quaternion[3] = fdm.quaternion.q1; - pkt.positionLLA[0] = fdm.latitude; - pkt.positionLLA[1] = fdm.longitude; - pkt.positionLLA[2] = fdm.altitude; - pkt.velNED[0] = fdm.speedN; - pkt.velNED[1] = fdm.speedE; - pkt.velNED[2] = fdm.speedD; + pkt.yprU[0] = 0.03; + pkt.yprU[1] = 0.03; + pkt.yprU[2] = 0.15; + + pkt.insStatus = 0x0306; + + pkt.posLla[0] = fdm.latitude; + pkt.posLla[1] = fdm.longitude; + pkt.posLla[2] = fdm.altitude; + pkt.velNed[0] = fdm.speedN; + pkt.velNed[1] = fdm.speedE; + pkt.velNed[2] = fdm.speedD; pkt.posU = 0.5; pkt.velU = 0.25; - const uint8_t header[] VN_PKT1_HEADER; - - write_to_autopilot((char *)&header, sizeof(header)); + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)sync_byte, 1); + write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header)); write_to_autopilot((char *)&pkt, sizeof(pkt)); - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; swab(&crc, &crc2, 2); write_to_autopilot((char *)&crc2, sizeof(crc2)); } -void VectorNav::send_packet2(void) +void VectorNav::send_ins_gnss_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet2 pkt {}; + struct VN_INS_gnss_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; struct timeval tv; simulation_timeval(&tv); - pkt.timeGPS = tv.tv_usec * 1000ULL; - - pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); - pkt.numGPS1Sats = 19; - pkt.GPS1Fix = 3; - pkt.GPS1posLLA[0] = fdm.latitude; - pkt.GPS1posLLA[1] = fdm.longitude; - pkt.GPS1posLLA[2] = fdm.altitude; - pkt.GPS1velNED[0] = fdm.speedN; - pkt.GPS1velNED[1] = fdm.speedE; - pkt.GPS1velNED[2] = fdm.speedD; - // pkt.GPS1DOP = - pkt.numGPS2Sats = 18; - pkt.GPS2Fix = 3; - - const uint8_t header[] VN_PKT2_HEADER; - - write_to_autopilot((char *)&header, sizeof(header)); + pkt.timeGps = tv.tv_usec * 1000ULL; + + pkt.numSats1 = 19; + pkt.fix1 = 3; + pkt.posLla1[0] = fdm.latitude; + pkt.posLla1[1] = fdm.longitude; + pkt.posLla1[2] = fdm.altitude; + pkt.velNed1[0] = fdm.speedN; + pkt.velNed1[1] = fdm.speedE; + pkt.velNed1[2] = fdm.speedD; + + pkt.posU1[0] = 1; + pkt.posU1[0] = 1; + pkt.posU1[0] = 1.5; + + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + // pkt.dop1 = + pkt.numSats2 = 18; + pkt.fix2 = 3; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)sync_byte, 1); + write_to_autopilot((char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header)); write_to_autopilot((char *)&pkt, sizeof(pkt)); - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); uint16_t crc2; @@ -203,14 +257,19 @@ void VectorNav::update(void) } uint32_t now = AP_HAL::micros(); - if (now - last_pkt1_us >= 20000) { - last_pkt1_us = now; - send_packet1(); + if (now - last_imu_pkt_us >= 20000) { + last_imu_pkt_us = now; + send_imu_packet(); } - - if (now - last_pkt2_us >= 200000) { - last_pkt2_us = now; - send_packet2(); + + if (now - last_ekf_pkt_us >= 20000) { + last_ekf_pkt_us = now; + send_ins_ekf_packet(); + } + + if (now - last_gnss_pkt_us >= 200000) { + last_gnss_pkt_us = now; + send_ins_gnss_packet(); } char receive_buf[50]; diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index 589539e5d9a8f3..77fde25625733a 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -41,12 +41,13 @@ class VectorNav : public SerialDevice { void update(void); private: - uint32_t last_pkt1_us; - uint32_t last_pkt2_us; - uint32_t last_type_us; + uint32_t last_imu_pkt_us; + uint32_t last_ekf_pkt_us; + uint32_t last_gnss_pkt_us; - void send_packet1(); - void send_packet2(); + void send_imu_packet(); + void send_ins_ekf_packet(); + void send_ins_gnss_packet(); void nmea_printf(const char *fmt, ...); };