void taskUpdateAttitude(void) {
imuUpdateAttitude();
}
void taskAttitude(timeUs_t currentTimeUs)
{
imuUpdateAttitude(currentTimeUs);
}
void updateState(const fdm_packet* pkt) {
static double last_timestamp = 0; // in seconds
static uint64_t last_realtime = 0; // in uS
static struct timespec last_ts; // last packet
struct timespec now_ts;
clock_gettime(CLOCK_MONOTONIC, &now_ts);
const uint64_t realtime_now = micros64_real();
if (realtime_now > last_realtime + 500*1e3) { // 500ms timeout
last_timestamp = pkt->timestamp;
last_realtime = realtime_now;
sendMotorUpdate();
return;
}
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
if (deltaSim < 0) { // don't use old packet
return;
}
int16_t x,y,z;
x = constrain(-pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE, -32767, 32767);
y = constrain(-pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE, -32767, 32767);
z = constrain(-pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE, -32767, 32767);
fakeAccSet(fakeAccDev, x, y, z);
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
x = constrain(pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG, -32767, 32767);
y = constrain(-pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG, -32767, 32767);
z = constrain(-pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG, -32767, 32767);
fakeGyroSet(fakeGyroDev, x, y, z);
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
#if defined(SKIP_IMU_CALC)
#if defined(SET_IMU_FROM_EULER)
// set from Euler
double qw = pkt->imu_orientation_quat[0];
double qx = pkt->imu_orientation_quat[1];
double qy = pkt->imu_orientation_quat[2];
double qz = pkt->imu_orientation_quat[3];
double ysqr = qy * qy;
double xf, yf, zf;
// roll (x-axis rotation)
double t0 = +2.0 * (qw * qx + qy * qz);
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
xf = atan2(t0, t1) * RAD2DEG;
// pitch (y-axis rotation)
double t2 = +2.0 * (qw * qy - qz * qx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
yf = asin(t2) * RAD2DEG; // from wiki
// yaw (z-axis rotation)
double t3 = +2.0 * (qw * qz + qx * qy);
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
zf = atan2(t3, t4) * RAD2DEG;
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
#else
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
#endif
#endif
#if defined(SIMULATOR_IMU_SYNC)
imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros());
#endif
if (deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts);
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
}
// printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
last_timestamp = pkt->timestamp;
last_realtime = micros64_real();
last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec;
pthread_mutex_unlock(&updateLock); // can send PWM output now
#if defined(SIMULATOR_GYROPID_SYNC)
pthread_mutex_unlock(&mainLoopLock); // can run main loop
#endif
}
