// class written for 3DM-CV7 imu (sheteshreyash) #include "SIM_MicroStrain.h" #include using namespace SITL; void MicroStrainCV7::send_ahrs_packet(void) { const auto &fdm = _sitl->state; MicroStrain_Packet packet; // Packet headers for GNSS-like structure packet.header[0] = 0x75; // Sync One packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x82; // GNSS-like Descriptor (For simulation purposes) // Quaternion orientation (Attitude Quaternion) float q1 = fdm.quaternion.q1; float q2 = fdm.quaternion.q2; float q3 = fdm.quaternion.q3; float q4 = fdm.quaternion.q4; float norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); if (norm > 0.0f) { q1 /= norm; q2 /= norm; q3 /= norm; q4 /= norm; } packet.payload[packet.payload_size++] = 0x04; // Field Size for Quaternion packet.payload[packet.payload_size++] = 0x03; // Quaternion Descriptor put_float(packet, q1); // w put_float(packet, q2); // x put_float(packet, q3); // y put_float(packet, q4); // z // Scaled Gyroscope (Angular rate in filtered data) packet.payload[packet.payload_size++] = 0x02; // Field Size for Scaled Gyroscope packet.payload[packet.payload_size++] = 0x05; // Scaled Gyroscope (Angular Rate) Descriptor put_float(packet, fdm.rollRate); // Angular rate X (roll) put_float(packet, fdm.pitchRate); // Angular rate Y (pitch) put_float(packet, fdm.yawRate); // Angular rate Z (yaw) // Scaled Acceleration (Linear Acceleration in filtered data) packet.payload[packet.payload_size++] = 0x02; // Field Size for Scaled Acceleration packet.payload[packet.payload_size++] = 0x04; // Acceleration Descriptor put_float(packet, fdm.xAccel); // Acceleration X put_float(packet, fdm.yAccel); // Acceleration Y put_float(packet, fdm.zAccel); // Acceleration Z // Scaled Magnetometer packet.payload[packet.payload_size++] = 0x02; // Field Size for Magnetometer packet.payload[packet.payload_size++] = 0x06; // Magnetometer Descriptor put_float(packet, fdm.bodyMagField.x); // Scaled X magnetic field put_float(packet, fdm.bodyMagField.y); // Scaled Y magnetic field put_float(packet, fdm.bodyMagField.z); // Scaled Z magnetic field // Euler Angles packet.payload[packet.payload_size++] = 0x10; // Field Size for Euler Angles packet.payload[packet.payload_size++] = 0x05; // Euler Angles Descriptor put_float(packet, fdm.rollDeg); // Roll put_float(packet, fdm.pitchDeg); // Pitch put_float(packet, fdm.yawDeg); // Yaw // Pressure Altitude (Simulated to mimic GNSS altitude field) packet.payload[packet.payload_size++] = 0x04; // Field Size for Pressure Altitude packet.payload[packet.payload_size++] = 0x21; // Pressure Altitude Descriptor put_float(packet, fdm.altitude); // Altitude (Simulated) // Packet length update packet.header[3] = packet.payload_size; // Send the packet send_packet(packet); } void MicroStrainCV7::send_filter_packet(void) { const auto &fdm = _sitl->state; MicroStrain_Packet packet; // Packet headers for CV7 packet.header[0] = 0x75; // Sync One packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x82; // Filter Descriptor // Quaternion orientation (Attitude Quaternion) float q1 = fdm.quaternion.q1; float q2 = fdm.quaternion.q2; float q3 = fdm.quaternion.q3; float q4 = fdm.quaternion.q4; float norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); if (norm > 0.0f) { q1 /= norm; q2 /= norm; q3 /= norm; q4 /= norm; } packet.payload[packet.payload_size++] = 0x04; // Field Size for Quaternion packet.payload[packet.payload_size++] = 0x03; // Quaternion Descriptor put_float(packet, q1); // w put_float(packet, q2); // x put_float(packet, q3); // y put_float(packet, q4); // z // Angular Rates (Compensated Angular Rate) packet.payload[packet.payload_size++] = 0x04; // Field Size for Angular Rates packet.payload[packet.payload_size++] = 0x0E; // Angular Rate Descriptor put_float(packet, fdm.rollRate); // Angular rate X (roll) put_float(packet, fdm.pitchRate); // Angular rate Y (pitch) put_float(packet, fdm.yawRate); // Angular rate Z (yaw) // Linear Acceleration (Accel Bias) packet.payload[packet.payload_size++] = 0x04; // Field Size for Linear Acceleration packet.payload[packet.payload_size++] = 0x07; // Acceleration Descriptor put_float(packet, fdm.xAccel); // Acceleration X put_float(packet, fdm.yAccel); // Acceleration Y put_float(packet, fdm.zAccel); // Acceleration Z // Magnetometer Data packet.payload[packet.payload_size++] = 0x02; // Field Size for Magnetometer packet.payload[packet.payload_size++] = 0x06; // Magnetometer Descriptor put_float(packet, fdm.bodyMagField.x); // Scaled X magnetic field put_float(packet, fdm.bodyMagField.y); // Scaled Y magnetic field put_float(packet, fdm.bodyMagField.z); // Scaled Z magnetic field // Euler Angles packet.payload[packet.payload_size++] = 0x10; // Field Size for Euler Angles packet.payload[packet.payload_size++] = 0x05; // Euler Angles Descriptor put_float(packet, fdm.rollDeg); // Roll put_float(packet, fdm.pitchDeg); // Pitch put_float(packet, fdm.yawDeg); // Yaw // // Gravity Vector // packet.payload[packet.payload_size++] = 0x06; // Field Size for Gravity Vector // packet.payload[packet.payload_size++] = 0x13; // Gravity Vector Descriptor // put_float(packet, fdm.gravity_x); // Gravity X // put_float(packet, fdm.gravity_y); // Gravity Y // put_float(packet, fdm.gravity_z); // Gravity Z // // Timestamp // uint32_t timestamp = fdm.timestamp_ms; // packet.payload[packet.payload_size++] = 0x04; // Field Size for Timestamp // packet.payload[packet.payload_size++] = 0x11; // Timestamp Descriptor // put_uint32(packet, timestamp); // Timestamp in milliseconds // Pressure Altitude packet.payload[packet.payload_size++] = 0x08; // Field Size for Pressure Altitude packet.payload[packet.payload_size++] = 0x21; // Pressure Altitude Descriptor put_float(packet, fdm.altitude); // Altitude // Packet length update packet.header[3] = packet.payload_size; // Send the packet send_packet(packet); }