mirror of https://github.com/ArduPilot/ardupilot
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:
parent
d4ca5fe868
commit
50980ee03e
libraries
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -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, ...);
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue