void IMU::SetYawPitchRoll(float yaw, float pitch, float roll, float compass_heading)
{
{
Synchronized sync(cIMUStateSemaphore);
this->yaw = yaw;
this->pitch = pitch;
this->roll = roll;
this->compass_heading = compass_heading;
}
UpdateYawHistory(this->yaw);
}
void IMU::SetYawPitchRoll(float yaw, float pitch, float roll, float compass_heading)
{
{
//Synchronized sync(cIMUStateSemaphore);
std::lock_guard<std::mutex> lock(mutex1);
this->yaw = yaw;
this->pitch = pitch;
this->roll = roll;
this->compass_heading = compass_heading;
}
UpdateYawHistory(this->yaw);
}
void IMU::SetYawPitchRoll(float yaw, float pitch, float roll, float compass_heading)
{
{
Synchronized sync(cIMUStateSemaphore);
float last_offset_corrected_yaw = GetYawUnsynchronized();
float last_yaw = this->yaw + 180.0f;
float curr_yaw = yaw + 180.0f;
float delta_yaw = curr_yaw - last_yaw;
this->last_yaw_rate = delta_yaw;
yaw_last_direction = 0;
if ( curr_yaw < last_yaw ) {
if ( delta_yaw < 180.0f ) {
yaw_last_direction = -1;
} else {
yaw_last_direction = 1;
}
} else if ( curr_yaw > last_yaw ) {
if ( delta_yaw > 180.0f ) {
yaw_last_direction = -1;
} else {
yaw_last_direction = 1;
}
}
this->yaw = yaw;
this->pitch = pitch;
this->roll = roll;
this->compass_heading = compass_heading;
float curr_offset_corrected_yaw = GetYawUnsynchronized();
if ( yaw_last_direction < 0 ) {
if ( ( curr_offset_corrected_yaw < 0.0f ) && ( last_offset_corrected_yaw >= 0.0f ) ) {
yaw_crossing_count--;
}
} else if ( yaw_last_direction > 0 ) {
if ( ( curr_offset_corrected_yaw >= 0.0f ) && ( last_offset_corrected_yaw < 0.0f ) ) {
yaw_crossing_count++;
}
}
}
UpdateYawHistory(this->yaw);
}
void IMUAdvanced::SetQuaternion( int16_t quat1, int16_t quat2, int16_t quat3, int16_t quat4,
int16_t accel_x, int16_t accel_y, int16_t accel_z,
int16_t mag_x, int16_t mag_y, int16_t mag_z,
float temp_c)
{
{
float q[4]; // Quaternion from IMU
float gravity[3]; // Gravity Vector
//float euler[3]; // Classic euler angle representation of quaternion
float ypr[3]; // Angles in "Tait-Bryan" (yaw/pitch/roll) format
float yaw_degrees;
float pitch_degrees;
float roll_degrees;
float linear_acceleration_x;
float linear_acceleration_y;
float linear_acceleration_z;
float q2[4];
float q_product[4];
float world_linear_acceleration_x;
float world_linear_acceleration_y;
float world_linear_acceleration_z;
// Convert 15-bit signed quaternion integral values to floats
q[0] = ((float)quat1) / 16384.0f;
q[1] = ((float)quat2) / 16384.0f;
q[2] = ((float)quat3) / 16384.0f;
q[3] = ((float)quat4) / 16384.0f;
// Fixup any quaternion values out of range
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
// below calculations are necessary for calculation of yaw/pitch/roll,
// and tilt-compensated compass heading
// calculate gravity vector
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
// calculate Euler angles
// This code is here for reference, and is commented out for performance reasons
//euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
//euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
//euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);
// calculate yaw/pitch/roll angles
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
// Convert yaw/pitch/roll angles to degreess, and remove calibrated offset
yaw_degrees = ypr[0] * (180.0/3.1415926);
pitch_degrees = ypr[1] * (180.0/3.1415926);
roll_degrees = ypr[2] * (180.0/3.1415926);
// Subtract offset, and handle potential 360 degree wrap-around
yaw_degrees -= yaw_offset_degrees;
if ( yaw_degrees < -180 ) yaw_degrees += 360;
if ( yaw_degrees > 180 ) yaw_degrees -= 360;
// calculate linear acceleration by
// removing the gravity component from raw acceleration values
// Note that this code assumes the acceleration full scale range
// is +/- 2 degrees
linear_acceleration_x = (float)(((float)accel_x) / (32768.0 / (float)accel_fsr_g)) - gravity[0];
linear_acceleration_y = (float)(((float)accel_y) / (32768.0 / (float)accel_fsr_g)) - gravity[1];
linear_acceleration_z = (float)(((float)accel_z) / (32768.0 / (float)accel_fsr_g)) - gravity[2];
// Calculate world-frame acceleration
q2[0] = 0;
q2[1] = linear_acceleration_x;
q2[2] = linear_acceleration_y;
q2[3] = linear_acceleration_z;
// Rotate linear acceleration so that it's relative to the world reference frame
// http://www.cprogramming.com/tutorial/3d/quaternions.html
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
// ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
// P_out = q * P_in * conj(q)
// - P_out is the output vector
// - q is the orientation quaternion
// - P_in is the input vector (a*aReal)
// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
// calculate quaternion product
// Quaternion multiplication is defined by:
// (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
// (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
// (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
// (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
q_product[0] = q[0]*q2[0] - q[1]*q2[1] - q[2]*q2[2] - q[3]*q2[3]; // new w
q_product[1] = q[0]*q2[1] + q[1]*q2[0] + q[2]*q2[3] - q[3]*q2[2]; // new x
q_product[2] = q[0]*q2[2] - q[1]*q2[3] + q[2]*q2[0] + q[3]*q2[1]; // new y
q_product[3] = q[0]*q2[3] + q[1]*q2[2] - q[2]*q2[1] + q[3]*q2[0]; // new z
float q_conjugate[4];
q_conjugate[0] = q[0];
q_conjugate[1] = -q[1];
q_conjugate[2] = -q[2];
q_conjugate[3] = -q[3];
float q_final[4];
q_final[0] = q_product[0]*q_conjugate[0] - q_product[1]*q_conjugate[1] - q_product[2]*q_conjugate[2] - q_product[3]*q_conjugate[3]; // new w
q_final[1] = q_product[0]*q_conjugate[1] + q_product[1]*q_conjugate[0] + q_product[2]*q_conjugate[3] - q_product[3]*q_conjugate[2]; // new x
q_final[2] = q_product[0]*q_conjugate[2] - q_product[1]*q_conjugate[3] + q_product[2]*q_conjugate[0] + q_product[3]*q_conjugate[1]; // new y
q_final[3] = q_product[0]*q_conjugate[3] + q_product[1]*q_conjugate[2] - q_product[2]*q_conjugate[1] + q_product[3]*q_conjugate[0]; // new z
world_linear_acceleration_x = q_final[1];
world_linear_acceleration_y = q_final[2];
world_linear_acceleration_z = q_final[3];
// Calculate tilt-compensated compass heading
float inverted_pitch = -ypr[1];
float roll = ypr[2];
float cos_roll = cos(roll);
float sin_roll = sin(roll);
float cos_pitch = cos(inverted_pitch);
float sin_pitch = sin(inverted_pitch);
float MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
float MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
float tilt_compensated_heading_radians = atan2(MAG_Y,MAG_X);
float tilt_compensated_heading_degrees = tilt_compensated_heading_radians * (180.0 / 3.1415926);
// Adjust compass for board orientation,
// and modify range from -180-180 to
// 0-360 degrees
tilt_compensated_heading_degrees -= 90.0;
if ( tilt_compensated_heading_degrees < 0 ) {
tilt_compensated_heading_degrees += 360;
}
this->yaw = yaw_degrees;
this->pitch = pitch_degrees;
this->roll = roll_degrees;
this->compass_heading = tilt_compensated_heading_degrees;
this->world_linear_accel_x = world_linear_acceleration_x;
this->world_linear_accel_y = world_linear_acceleration_y;
this->world_linear_accel_z = world_linear_acceleration_z;
this->temp_c = temp_c;
UpdateYawHistory(this->yaw);
UpdateWorldLinearAccelHistory( world_linear_acceleration_x, world_linear_acceleration_y, world_linear_acceleration_z);
}
}
int AHRS::DecodePacketHandler( char *received_data, int bytes_remaining )
{
float yaw, pitch, roll, compass_heading;
float altitude, fused_heading, linear_accel_x, linear_accel_y, linear_accel_z;
float mpu_temp;
int16_t raw_mag_x, raw_mag_y, raw_mag_z;
int16_t cal_mag_x, cal_mag_y, cal_mag_z;
float mag_norm_ratio, mag_norm_scalar;
int16_t quat_w, quat_x, quat_y, quat_z;
float baro_pressure, baro_temp;
uint8_t op_status, sensor_status;
uint8_t cal_status, selftest_status;
int packet_length = AHRSProtocol::decodeAHRSUpdate( received_data, bytes_remaining,
yaw,
pitch,
roll,
compass_heading,
altitude,
fused_heading,
linear_accel_x,
linear_accel_y,
linear_accel_z,
mpu_temp,
raw_mag_x,
raw_mag_y,
raw_mag_z,
cal_mag_x,
cal_mag_y,
cal_mag_z,
mag_norm_ratio,
mag_norm_scalar,
quat_w,
quat_x,
quat_y,
quat_z,
baro_pressure,
baro_temp,
op_status,
sensor_status,
cal_status,
selftest_status);
if (packet_length > 0) {
/* Update base IMU class variables */
this->yaw = yaw;
this->pitch = pitch;
this->roll = roll;
this->compass_heading = compass_heading;
UpdateYawHistory(yaw);
/* Update AHRS class variables */
// 9-axis data
this->fused_heading = fused_heading;
// Gravity-corrected linear acceleration (world-frame)
this->world_linear_accel_x = linear_accel_x;
this->world_linear_accel_y = linear_accel_y;
this->world_linear_accel_z = linear_accel_z;
// Gyro/Accelerometer Die Temperature
this->mpu_temp_c = mpu_temp;
// Barometric Pressure/Altitude
this->altitude = altitude;
this->barometric_pressure = baro_pressure;
this->baro_sensor_temp_c = baro_temp;
// Magnetometer Data
this->cal_mag_x = cal_mag_x;
this->cal_mag_y = cal_mag_y;
this->cal_mag_z = cal_mag_z;
this->mag_field_norm_ratio = mag_field_norm_ratio;
// Status/Motion Detection
this->is_moving =
(((sensor_status &
NAVX_SENSOR_STATUS_MOVING) != 0)
? true : false);
this->is_rotating =
(((sensor_status &
NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
? true : false);
this->altitude_valid =
(((sensor_status &
NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
? true : false);
this->is_magnetometer_calibrated =
(((cal_status &
NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
? true : false);
this->magnetic_disturbance =
(((sensor_status &
NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
? true : false);
UpdateDisplacement( this->world_linear_accel_x,
this->world_linear_accel_y,
update_rate_hz,
this->is_moving);
}
return packet_length;
}
