static void main_run_ins(uint8_t data_valid) {
struct timespec now;
clock_gettime(TIMER, &now);
double dt_imu_freq = 0.001953125; // 1/512; // doesn't work?
ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
if(MAG_READY(data_valid)){
ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
}
#if UPDATE_WITH_GRAVITY
if(CLOSE_TO_GRAVITY(imu_float.accel)){
// use the gravity as reference
ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3);
}
#endif
if(GPS_READY(data_valid)){
ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise);
}
print_estimator_state(absTime(time_diff(now, start)));
}
static struct raw_log_entry next_GPS(int file_descriptor){
uint8_t read_ok;
struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
while ((read_ok)&&(!GPS_READY(e.message.valid_sensors))) {
e = read_raw_log_entry(file_descriptor, &read_ok);
}
return e;
}
//static uint8_t main_dialog_with_io_proc() {
static void main_dialog_with_io_proc() {
DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message);
uint8_t crc_valid;
// for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
spi_link_send(&message_out, sizeof(struct AutopilotMessageCRCFrame), &message_in, &crc_valid);
main_rawlog_dump(&message_in.payload.msg_up);
if(GPS_READY(message_in.payload.msg_up.valid_sensors)){printf("GPS!\n");}
/*struct AutopilotMessageVIUp *in = &message_in.payload.msg_up;
if(IMU_READY(in->valid_sensors)){
COPY_RATES_ACCEL_TO_IMU_FLOAT(in);
}
if(MAG_READY(in->valid_sensors)){
COPY_MAG_TO_IMU_FLOAT(in);
#if PRINT_MAG
printmag();
#endif
}
if(BARO_READY(in->valid_sensors)){
baro_measurement = in->pressure_absolute;
}
if(GPS_READY(in->valid_sensors)){
COPY_GPS_TO_IMU(in);
#if PRINT_GPS
printgps();
#endif
}
return in->valid_sensors;*/
}
static void main_run_ins(uint8_t data_valid) {
double dt_imu_freq = 0.001953125; // 1/512; // doesn't work?
ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
if(MAG_READY(data_valid)){
ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), magnetometer_noise.norm());
}
#if UPDATE_WITH_GRAVITY
if(CLOSE_TO_GRAVITY(imu_float.accel)){
// use the gravity as reference
ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), accelerometer_noise.norm());
}
#endif /* UPDATE_WITH_GRAVITY */
if(BARO_READY(data_valid)){
ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
} // comment out multiple lines */
if(GPS_READY(data_valid)){
ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
} // comment out multiple lines */
}
static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){
int imu_measurements = 0, // => Gyro + Accel
magnetometer_measurements = 0,
baro_measurements = 0,
gps_measurements = 0; // only the position
struct DoubleMat33 attitude_profile_matrix, sigmaB; // the attitude profile matrix is often called "B"
struct Orientation_Measurement gravity,
magneto,
fake;
struct DoubleQuat q_ned2body, sigma_q;
/* Prepare the attitude profile matrix */
FLOAT_MAT33_ZERO(attitude_profile_matrix);
FLOAT_MAT33_ZERO(sigmaB);
// for faster converging, but probably more rounding error
#define MEASUREMENT_WEIGHT_SCALE 10
/* set the gravity measurement */
VECT3_ASSIGN(gravity.reference_direction, 0,0,-1);
gravity.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(imu_frequency); // originally 1/(imu_frequency*gravity.norm()
//gravity.weight_of_the_measurement = 1;
/* set the magneto - measurement */
EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction);
magneto.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(mag_frequency); // originally 1/(mag_frequency*reference_direction.norm()
//magneto.weight_of_the_measurement = 1;
uint8_t read_ok;
#if WITH_GPS
struct raw_log_entry e = next_GPS(file_descriptor);
#else /* WITH_GPS */
struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
pos_cov_0 = Vector3d::Ones()*100;
speed_0_ecef = Vector3d::Zero();
speed_cov_0 = Vector3d::Ones();
#endif /* WITH_GPS */
#ifdef EKNAV_FROM_LOG_DEBUG
int imu_ready = 0,
mag_ready = 0,
baro_ready = 0,
gps_ready = 0;
#endif /* EKNAV_FROM_LOG_DEBUG */
for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){
if(IMU_READY(e.message.valid_sensors)){
imu_measurements++;
// update the estimated bias
bias_0 = NEW_MEAN(bias_0, RATES_BFP_AS_VECTOR3D(e.message.gyro), imu_measurements);
// update the attitude profile matrix
ACCELS_FLOAT_OF_BFP(gravity.measured_direction,e.message.accel);
add_orientation_measurement(&attitude_profile_matrix, gravity);
}
if(MAG_READY(e.message.valid_sensors)){
magnetometer_measurements++;
// update the attitude profile matrix
MAGS_FLOAT_OF_BFP(magneto.measured_direction,e.message.mag);
add_orientation_measurement(&attitude_profile_matrix, magneto);
// now, generate fake measurement with the last gravity measurement
fake = fake_orientation_measurement(gravity, magneto);
add_orientation_measurement(&attitude_profile_matrix, fake);
}
if(BARO_READY(e.message.valid_sensors)){
baro_measurements++;
// TODO: Fix it!
//NEW_MEAN(baro_0_height, BARO_FLOAT_OF_BFP(e.message.pressure_absolute), baro_measurements);
baro_0_height = (baro_0_height*(baro_measurements-1)+BARO_FLOAT_OF_BFP(e.message.pressure_absolute))/baro_measurements;
}
if(GPS_READY(e.message.valid_sensors)){
gps_measurements++;
// update the estimated bias
pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_pos)/100, gps_measurements);
speed_0_ecef = NEW_MEAN(speed_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_vel)/100, gps_measurements);
}
#ifdef EKNAV_FROM_LOG_DEBUG
if(imu_ready==0){
if(!NOT_ENOUGH_IMU_MEASUREMENTS(imu_measurements)){
printf("IMU READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
imu_ready = 1;
}
}
if(mag_ready==0){
if(!NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(magnetometer_measurements)){
printf("MAG READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
mag_ready = 1;
}
}
if(baro_ready==0){
if(!NOT_ENOUGH_BARO_MEASUREMENTS(baro_measurements)){
printf("BARO READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
baro_ready = 1;
}
}
if(gps_ready==0){
if(!NOT_ENOUGH_GPS_MEASUREMENTS(gps_measurements)){
printf("GPS READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
gps_ready = 1;
}
}
#endif /* EKNAV_FROM_LOG_DEBUG */
}
// setting the covariance
gravity.weight_of_the_measurement *= imu_measurements;
VECTOR_AS_VECT3(gravity.measured_direction, accelerometer_noise);
magneto.weight_of_the_measurement *= magnetometer_measurements;
VECTOR_AS_VECT3(magneto.measured_direction, magnetometer_noise);
add_set_of_three_measurements(&sigmaB, gravity, magneto);
#ifdef EKNAV_FROM_LOG_DEBUG
DISPLAY_FLOAT_RMAT(" B", attitude_profile_matrix);
DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
#endif /* EKNAV_FROM_LOG_DEBUG */
// setting the initial orientation
q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q);
orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
baro_0_height += pos_0_ecef.norm();
struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
#if WITH_GPS
pos_cov_0 = 10*gps_pos_noise / gps_measurements;
speed_cov_0 = 10*gps_speed_noise / gps_measurements;
#endif // WITH_GPS
return e;
}
