Skip to content

Commit

Permalink
AP_ExternalAHRS_VectorNav: Update SIM to match new message definitions
Browse files Browse the repository at this point in the history 
Redefine messages to 3 INS packets, and send as appropriate
  • Loading branch information
@lashVN
lashVN committed Jul 19, 2024
1 parent 35a71f6 commit efb74b0
Show file tree
Hide file tree
Showing 2 changed files with 139 additions and 79 deletions.
207 changes: 133 additions & 74 deletions libraries/SITL/SIM_VectorNav.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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];
Expand Down
11 changes: 6 additions & 5 deletions libraries/SITL/SIM_VectorNav.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, ...);
};

Expand Down

0 comments on commit efb74b0

Please sign in to comment.