Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 22:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS_VectorNav: Update SIM to match new message definitions

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -551,7 +551,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
// @Field: GY: Rotation rate Y-axis // @Field: GY: Rotation rate Y-axis
// @Field: GZ: Rotation rate Z-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", "sdPGGGoooEEE", "F00000000000",
"Qfffffffffff", "Qfffffffffff",
AP_HAL::micros64(), AP_HAL::micros64(),
@ -587,7 +587,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) {
// @Field: PU: Pitch uncertainty // @Field: PU: Pitch uncertainty
// @Field: RU: Roll 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", "s----dddddd", "F0000000000",
"Qffffffffff", "Qffffffffff",
AP_HAL::micros64(), 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; const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b;
last_pkt2_ms = AP_HAL::millis(); 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.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]};
state.have_quaternion = true; 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: PU: Pitch uncertainty
// @Field: RU: Roll 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", "s----dddddd", "F0000000000",
"Qffffffffff", "Qffffffffff",
now, now,
@ -652,8 +652,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
// @Field: PosU: Filter estimated position uncertainty // @Field: PosU: Filter estimated position uncertainty
// @Field: VelU: Filter estimated Velocity uncertainty // @Field: VelU: Filter estimated Velocity uncertainty
AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", AP::logger().WriteStreaming("EAHK", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
"s-ddmnnndn", "F00000000", "s-ddmnnndn", "F000000000",
"QHdddfffff", "QHdddfffff",
now, now,
pkt.insStatus, pkt.insStatus,
@ -670,7 +670,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) {
last_pkt3_ms = AP_HAL::millis(); last_pkt3_ms = AP_HAL::millis();
latest_ins_gnss_packet = &pkt; *latest_ins_gnss_packet = pkt;
// get ToW in milliseconds // get ToW in milliseconds
gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL);

6
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
View File

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

199
libraries/SITL/SIM_VectorNav.cpp
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 uncompAccel[3];
float uncompAngRate[3]; float uncompAngRate[3];
float pressure;
float mag[3]; float mag[3];
float accel[3]; float temp;
float gyro[3]; 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 ypr[3];
float quaternion[4]; float quaternion[4];
float yprU[3]; float yprU[3];
double positionLLA[3]; uint16_t insStatus;
float velNED[3]; double posLla[3];
float velNed[3];
float posU; float posU;
float velU; float velU;
}; };
constexpr uint8_t VN_INS_ekf_packet_sim::header[];
struct PACKED VN_INS_packet2 { struct PACKED VN_INS_gnss_packet_sim {
uint64_t timeGPS; static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00};
float temp; uint64_t timeStartup;
uint8_t numGPS1Sats; uint64_t timeGps;
uint8_t GPS1Fix; uint8_t numSats1;
double GPS1posLLA[3]; uint8_t fix1;
float GPS1velNED[3]; double posLla1[3];
float GPS1DOP[7]; float velNed1[3];
uint8_t numGPS2Sats; float posU1[3];
uint8_t GPS2Fix; float velU1;
float dop1[7];
uint8_t numSats2;
uint8_t fix2;
}; };
constexpr uint8_t VN_INS_gnss_packet_sim::header[];
#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 }
/* /*
get timeval using simulation time get timeval using simulation time
@ -80,36 +92,62 @@ static void simulation_timeval(struct timeval *tv)
tv->tv_usec = new_usec % 1000000ULL; tv->tv_usec = new_usec % 1000000ULL;
} }
void VectorNav::send_packet1(void) void VectorNav::send_imu_packet(void)
{ {
const auto &fdm = _sitl->state; 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[0] = fdm.xAccel;
pkt.uncompAccel[1] = fdm.yAccel; pkt.uncompAccel[1] = fdm.yAccel;
pkt.uncompAccel[2] = fdm.zAccel; pkt.uncompAccel[2] = fdm.zAccel;
const float gyro_noise = 0.05;
pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float()); pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float());
pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float()); pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float());
pkt.uncompAngRate[2] = radians(fdm.yawRate + 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[0] = fdm.bodyMagField.x*0.001;
pkt.mag[1] = fdm.bodyMagField.y*0.001; pkt.mag[1] = fdm.bodyMagField.y*0.001;
pkt.mag[2] = fdm.bodyMagField.z*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.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude);
pkt.accel[1] = fdm.yAccel;
pkt.accel[2] = fdm.zAccel; const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise);
pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); 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[0] = fdm.yawDeg;
pkt.ypr[1] = fdm.pitchDeg; pkt.ypr[1] = fdm.pitchDeg;
@ -120,58 +158,74 @@ void VectorNav::send_packet1(void)
pkt.quaternion[2] = fdm.quaternion.q4; pkt.quaternion[2] = fdm.quaternion.q4;
pkt.quaternion[3] = fdm.quaternion.q1; pkt.quaternion[3] = fdm.quaternion.q1;
pkt.positionLLA[0] = fdm.latitude; pkt.yprU[0] = 0.03;
pkt.positionLLA[1] = fdm.longitude; pkt.yprU[1] = 0.03;
pkt.positionLLA[2] = fdm.altitude; pkt.yprU[2] = 0.15;
pkt.velNED[0] = fdm.speedN;
pkt.velNED[1] = fdm.speedE; pkt.insStatus = 0x0306;
pkt.velNED[2] = fdm.speedD;
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.posU = 0.5;
pkt.velU = 0.25; pkt.velU = 0.25;
const uint8_t header[] VN_PKT1_HEADER; const uint8_t sync_byte = 0xFA;
write_to_autopilot((char *)&sync_byte, 1);
write_to_autopilot((char *)&header, sizeof(header)); write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header));
write_to_autopilot((char *)&pkt, sizeof(pkt)); 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); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
uint16_t crc2; uint16_t crc2;
swab(&crc, &crc2, 2); swab(&crc, &crc2, 2);
write_to_autopilot((char *)&crc2, sizeof(crc2)); write_to_autopilot((char *)&crc2, sizeof(crc2));
} }
void VectorNav::send_packet2(void) void VectorNav::send_ins_gnss_packet(void)
{ {
const auto &fdm = _sitl->state; 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; struct timeval tv;
simulation_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.numSats1 = 19;
pkt.numGPS1Sats = 19; pkt.fix1 = 3;
pkt.GPS1Fix = 3; pkt.posLla1[0] = fdm.latitude;
pkt.GPS1posLLA[0] = fdm.latitude; pkt.posLla1[1] = fdm.longitude;
pkt.GPS1posLLA[1] = fdm.longitude; pkt.posLla1[2] = fdm.altitude;
pkt.GPS1posLLA[2] = fdm.altitude; pkt.velNed1[0] = fdm.speedN;
pkt.GPS1velNED[0] = fdm.speedN; pkt.velNed1[1] = fdm.speedE;
pkt.GPS1velNED[1] = fdm.speedE; pkt.velNed1[2] = fdm.speedD;
pkt.GPS1velNED[2] = fdm.speedD;
// pkt.GPS1DOP =
pkt.numGPS2Sats = 18;
pkt.GPS2Fix = 3;
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)); 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); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
uint16_t crc2; uint16_t crc2;
@ -203,14 +257,19 @@ void VectorNav::update(void)
} }
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
if (now - last_pkt1_us >= 20000) { if (now - last_imu_pkt_us >= 20000) {
last_pkt1_us = now; last_imu_pkt_us = now;
send_packet1(); send_imu_packet();
} }
if (now - last_pkt2_us >= 200000) { if (now - last_ekf_pkt_us >= 20000) {
last_pkt2_us = now; last_ekf_pkt_us = now;
send_packet2(); send_ins_ekf_packet();
}
if (now - last_gnss_pkt_us >= 200000) {
last_gnss_pkt_us = now;
send_ins_gnss_packet();
} }
char receive_buf[50]; char receive_buf[50];

11
libraries/SITL/SIM_VectorNav.h
View File

@ -41,12 +41,13 @@ public:
void update(void); void update(void);
private: private:
uint32_t last_pkt1_us; uint32_t last_imu_pkt_us;
uint32_t last_pkt2_us; uint32_t last_ekf_pkt_us;
uint32_t last_type_us; uint32_t last_gnss_pkt_us;
void send_packet1(); void send_imu_packet();
void send_packet2(); void send_ins_ekf_packet();
void send_ins_gnss_packet();
void nmea_printf(const char *fmt, ...); void nmea_printf(const char *fmt, ...);
}; };