AP_ExternalAHRS_VectorNav: Update SIM to match new message definitions

Redefine messages to 3 INS packets, and send as appropriate
Resolve SITL failures due to too-long logger message headers
This commit is contained in:
BLash 2024-07-18 19:27:08 -05:00 committed by Andrew Tridgell
parent d4ca5fe868
commit 50980ee03e
4 changed files with 145 additions and 85 deletions

View File

@ -551,7 +551,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
// @Field: GY: Rotation rate Y-axis
// @Field: GZ: Rotation rate Z-axis
AP::logger().WriteStreaming("EAHIM", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ",
AP::logger().WriteStreaming("EAHI", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ",
"sdPGGGoooEEE", "F00000000000",
"Qfffffffffff",
AP_HAL::micros64(),
@ -587,7 +587,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) {
// @Field: PU: Pitch uncertainty
// @Field: RU: Roll uncertainty
AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
"s----dddddd", "F0000000000",
"Qffffffffff",
AP_HAL::micros64(),
@ -602,7 +602,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b;
last_pkt2_ms = AP_HAL::millis();
latest_ins_ekf_packet = &pkt;
*latest_ins_ekf_packet = pkt;
state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]};
state.have_quaternion = true;
@ -631,7 +631,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
// @Field: PU: Pitch uncertainty
// @Field: RU: Roll uncertainty
AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
"s----dddddd", "F0000000000",
"Qffffffffff",
now,
@ -652,8 +652,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
// @Field: PosU: Filter estimated position uncertainty
// @Field: VelU: Filter estimated Velocity uncertainty
AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
"s-ddmnnndn", "F00000000",
AP::logger().WriteStreaming("EAHK", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
"s-ddmnnndn", "F000000000",
"QHdddfffff",
now,
pkt.insStatus,
@ -670,7 +670,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) {
last_pkt3_ms = AP_HAL::millis();
latest_ins_gnss_packet = &pkt;
*latest_ins_gnss_packet = pkt;
// get ToW in milliseconds
gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL);

View File

@ -73,9 +73,9 @@ private:
uint16_t pktoffset;
uint16_t bufsize;
struct VN_imu_packet const *latest_imu_packet = nullptr;
struct VN_INS_ekf_packet const *latest_ins_ekf_packet = nullptr;
struct VN_INS_gnss_packet const *latest_ins_gnss_packet = nullptr;
struct VN_imu_packet *latest_imu_packet = nullptr;
struct VN_INS_ekf_packet *latest_ins_ekf_packet = nullptr;
struct VN_INS_gnss_packet *latest_ins_gnss_packet = nullptr;
uint32_t last_pkt1_ms = UINT32_MAX;
uint32_t last_pkt2_ms = UINT32_MAX;

View File

@ -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;
};
constexpr uint8_t VN_INS_ekf_packet_sim::header[];
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;
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.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;
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;
const uint8_t header[] VN_PKT2_HEADER;
pkt.posU1[0] = 1;
pkt.posU1[0] = 1;
pkt.posU1[0] = 1.5;
write_to_autopilot((char *)&header, sizeof(header));
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];

View File

@ -41,12 +41,13 @@ public:
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, ...);
};