static void position_estimation_position_integration(position_estimator_t *pos_est) { int32_t i; float dt = pos_est->ahrs->dt; quat_t qvel_bf,qvel; qvel.s = 0; for (i = 0; i < 3; i++) { qvel.v[i] = pos_est->vel[i]; } qvel_bf = quaternions_global_to_local(pos_est->ahrs->qe, qvel); for (i = 0; i < 3; i++) { pos_est->vel_bf[i] = qvel_bf.v[i]; pos_est->vel_bf[i] = pos_est->vel_bf[i] * (1.0f - (VEL_DECAY * dt)) + pos_est->ahrs->linear_acc[i] * dt; } // calculate velocity in global frame // vel = qe *vel_bf * qe - 1 qvel_bf.s = 0.0f; qvel_bf.v[0] = pos_est->vel_bf[0]; // TODO: replace this by quat_rotate_vector() qvel_bf.v[1] = pos_est->vel_bf[1]; qvel_bf.v[2] = pos_est->vel_bf[2]; qvel = quaternions_local_to_global(pos_est->ahrs->qe, qvel_bf); pos_est->vel[0] = qvel.v[0]; pos_est->vel[1] = qvel.v[1]; pos_est->vel[2] = qvel.v[2]; // TODO: replace this by quat_rotate_vector() for (i = 0; i < 3; i++) { pos_est->local_position.pos[i] = pos_est->local_position.pos[i] * (1.0f - (POS_DECAY * dt)) + pos_est->vel[i] * dt; pos_est->local_position.heading = coord_conventions_get_yaw(pos_est->ahrs->qe); } }
static void acoustic_set_waypoint_command(audio_t* audio_data) { quat_t qtmp1, qtmp2; float target_vect_global[3]; float speed_gain; qtmp1 = quaternions_create_from_vector(target_vect); qtmp2 = quaternions_local_to_global(attitude_previous, qtmp1); //unit vector to target target_vect_global[0] = qtmp2.v[0]; target_vect_global[1] = qtmp2.v[1]; target_vect_global[2] = qtmp2.v[2]; //distance to target on ground (computed from the hight of robot) //speed_gain = f_abs(position_previous[2]/target_vect_global[2]); //speed_gain = f_abs(1.0-target_vect_global[2])*15.0 speed_gain = maths_f_abs(quick_trig_acos(target_vect_global[2])) * 20.0f; if (speed_gain > 20.0f) { speed_gain = 20.0f; } else if (speed_gain < 3.0f) { speed_gain = 0.0f; } audio_data->waypoint_handler->waypoint_hold_coordinates.pos[0] = speed_gain * target_vect_global[0] + position_previous[0]; audio_data->waypoint_handler->waypoint_hold_coordinates.pos[1] = speed_gain * target_vect_global[1] + position_previous[1]; //centralData->controls_nav.theading=audio_data->azimuth*PI/180; if (speed_gain > 3.0f) { audio_data->waypoint_handler->waypoint_hold_coordinates.heading = atan2(target_vect_global[Y], target_vect_global[X]); } }
void simulation_update(simulation_model_t *sim) { int32_t i; quat_t qtmp1, qvel_bf, qed; const quat_t front = {.s = 0.0f, .v = {1.0f, 0.0f, 0.0f}}; const quat_t up = {.s = 0.0f, .v = {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}}; uint32_t now = time_keeper_get_micros(); sim->dt = (now - sim->last_update) / 1000000.0f; if (sim->dt > 0.1f) { sim->dt = 0.1f; } sim->last_update = now; // compute torques and forces based on servo commands forces_from_servos_diag_quad(sim); // integrate torques to get simulated gyro rates (with some damping) sim->rates_bf[0] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[0] + sim->dt * sim->torques_bf[0] / sim->vehicle_config.roll_pitch_momentum, 10.0f); sim->rates_bf[1] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[1] + sim->dt * sim->torques_bf[1] / sim->vehicle_config.roll_pitch_momentum, 10.0f); sim->rates_bf[2] = maths_clip((1.0f - 0.1f * sim->dt) * sim->rates_bf[2] + sim->dt * sim->torques_bf[2] / sim->vehicle_config.yaw_momentum, 10.0f); for (i = 0; i < 3; i++) { qtmp1.v[i] = 0.5f * sim->rates_bf[i]; } qtmp1.s = 0; // apply step rotation qed = quaternions_multiply(sim->ahrs.qe,qtmp1); // TODO: correct this formulas when using the right scales sim->ahrs.qe.s = sim->ahrs.qe.s + qed.s * sim->dt; sim->ahrs.qe.v[0] += qed.v[0] * sim->dt; sim->ahrs.qe.v[1] += qed.v[1] * sim->dt; sim->ahrs.qe.v[2] += qed.v[2] * sim->dt; sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe); sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up); sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front); // velocity and position integration // check altitude - if it is lower than 0, clamp everything (this is in NED, assuming negative altitude) if (sim->local_position.pos[Z] >0) { sim->vel[Z] = 0.0f; sim->local_position.pos[Z] = 0.0f; // simulate "acceleration" caused by contact force with ground, compensating gravity for (i = 0; i < 3; i++) { sim->lin_forces_bf[i] = sim->ahrs.up_vec.v[i] * sim->vehicle_config.total_mass * sim->vehicle_config.sim_gravity; } // slow down... (will make velocity slightly inconsistent until next update cycle, but shouldn't matter much) for (i = 0; i < 3; i++) { sim->vel_bf[i] = 0.95f * sim->vel_bf[i]; } //upright sim->rates_bf[0] = sim->ahrs.up_vec.v[1]; sim->rates_bf[1] = - sim->ahrs.up_vec.v[0]; sim->rates_bf[2] = 0; } sim->ahrs.qe = quaternions_normalise(sim->ahrs.qe); sim->ahrs.up_vec = quaternions_global_to_local(sim->ahrs.qe, up); sim->ahrs.north_vec = quaternions_global_to_local(sim->ahrs.qe, front); for (i = 0; i < 3; i++) { qtmp1.v[i] = sim->vel[i]; } qtmp1.s = 0.0f; qvel_bf = quaternions_global_to_local(sim->ahrs.qe, qtmp1); float acc_bf[3]; for (i = 0; i < 3; i++) { sim->vel_bf[i] = qvel_bf.v[i]; // following the convention in the IMU, this is the acceleration due to force, as measured sim->ahrs.linear_acc[i] = sim->lin_forces_bf[i] / sim->vehicle_config.total_mass; // this is the "clean" acceleration without gravity acc_bf[i] = sim->ahrs.linear_acc[i] - sim->ahrs.up_vec.v[i] * sim->vehicle_config.sim_gravity; sim->vel_bf[i] = sim->vel_bf[i] + acc_bf[i] * sim->dt; } // calculate velocity in global frame // vel = qe *vel_bf * qe - 1 qvel_bf.s = 0.0f; qvel_bf.v[0] = sim->vel_bf[0]; qvel_bf.v[1] = sim->vel_bf[1]; qvel_bf.v[2] = sim->vel_bf[2]; qtmp1 = quaternions_local_to_global(sim->ahrs.qe, qvel_bf); sim->vel[0] = qtmp1.v[0]; sim->vel[1] = qtmp1.v[1]; sim->vel[2] = qtmp1.v[2]; for (i = 0; i < 3; i++) { sim->local_position.pos[i] = sim->local_position.pos[i] + sim->vel[i] * sim->dt; } // fill in simulated IMU values for (i = 0;i < 3; i++) { sim->imu->raw_gyro.data[sim->imu->calib_gyro.axis[i]] = (sim->rates_bf[i] * sim->calib_gyro.scale_factor[i] + sim->calib_gyro.bias[i]) * sim->calib_gyro.orientation[i]; sim->imu->raw_accelero.data[sim->imu->calib_accelero.axis[i]] = ((sim->lin_forces_bf[i] / sim->vehicle_config.total_mass / sim->vehicle_config.sim_gravity) * sim->calib_accelero.scale_factor[i] + sim->calib_accelero.bias[i]) * sim->calib_accelero.orientation[i]; sim->imu->raw_compass.data[sim->imu->calib_compass.axis[i]] = ((sim->ahrs.north_vec.v[i] ) * sim->calib_compass.scale_factor[i] + sim->calib_compass.bias[i])* sim->calib_compass.orientation[i]; } sim->local_position.heading = coord_conventions_get_yaw(sim->ahrs.qe); } void simulation_simulate_barometer(simulation_model_t *sim) { sim->pressure->altitude = sim->local_position.origin.altitude - sim->local_position.pos[Z]; sim->pressure->vario_vz = sim->vel[Z]; sim->pressure->last_update = time_keeper_get_millis(); sim->pressure->altitude_offset = 0; } void simulation_simulate_gps(simulation_model_t *sim) { global_position_t gpos = coord_conventions_local_to_global_position(sim->local_position); sim->gps->altitude = gpos.altitude; sim->gps->latitude = gpos.latitude; sim->gps->longitude = gpos.longitude; sim->gps->time_last_msg = time_keeper_get_millis(); sim->gps->time_last_posllh_msg = time_keeper_get_millis(); sim->gps->time_last_velned_msg = time_keeper_get_millis(); sim->gps->north_speed = sim->vel[X]; sim->gps->east_speed = sim->vel[Y]; sim->gps->vertical_speed = sim->vel[Z]; sim->gps->status = GPS_OK; sim->gps->healthy = true; }
void qfilter_update(qfilter_t *qf) { static uint32_t convergence_update_count = 0; float omc[3], omc_mag[3] , tmp[3], snorm, norm, s_acc_norm, acc_norm, s_mag_norm, mag_norm; quat_t qed, qtmp1, up, up_bf; quat_t mag_global, mag_corrected_local; quat_t front_vec_global = { .s = 0.0f, .v = {1.0f, 0.0f, 0.0f} }; float kp = qf->kp; float kp_mag = qf->kp_mag; float ki = qf->ki; float ki_mag = qf->ki_mag; // Update time in us float now_us = time_keeper_get_micros(); // Delta t in seconds float dt = 1e-6 * (float)(now_us - qf->ahrs->last_update); // Write to ahrs structure qf->ahrs->dt = dt; qf->ahrs->last_update = now_us; // up_bf = qe^-1 *(0,0,0,-1) * qe up.s = 0; up.v[X] = UPVECTOR_X; up.v[Y] = UPVECTOR_Y; up.v[Z] = UPVECTOR_Z; up_bf = quaternions_global_to_local(qf->ahrs->qe, up); // calculate norm of acceleration vector s_acc_norm = qf->imu->scaled_accelero.data[X] * qf->imu->scaled_accelero.data[X] + qf->imu->scaled_accelero.data[Y] * qf->imu->scaled_accelero.data[Y] + qf->imu->scaled_accelero.data[Z] * qf->imu->scaled_accelero.data[Z]; if ( (s_acc_norm > 0.7f * 0.7f) && (s_acc_norm < 1.3f * 1.3f) ) { // approximate square root by running 2 iterations of newton method acc_norm = maths_fast_sqrt(s_acc_norm); tmp[X] = qf->imu->scaled_accelero.data[X] / acc_norm; tmp[Y] = qf->imu->scaled_accelero.data[Y] / acc_norm; tmp[Z] = qf->imu->scaled_accelero.data[Z] / acc_norm; // omc = a x up_bf.v CROSS(tmp, up_bf.v, omc); } else { omc[X] = 0; omc[Y] = 0; omc[Z] = 0; } // Heading computation // transfer qtmp1 = quaternions_create_from_vector(qf->imu->scaled_compass.data); mag_global = quaternions_local_to_global(qf->ahrs->qe, qtmp1); // calculate norm of compass vector //s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]) + SQR(mag_global.v[Z]); s_mag_norm = SQR(mag_global.v[X]) + SQR(mag_global.v[Y]); if ( (s_mag_norm > 0.004f * 0.004f) && (s_mag_norm < 1.8f * 1.8f) ) { mag_norm = maths_fast_sqrt(s_mag_norm); mag_global.v[X] /= mag_norm; mag_global.v[Y] /= mag_norm; mag_global.v[Z] = 0.0f; // set z component in global frame to 0 // transfer magneto vector back to body frame qf->ahrs->north_vec = quaternions_global_to_local(qf->ahrs->qe, front_vec_global); mag_corrected_local = quaternions_global_to_local(qf->ahrs->qe, mag_global); // omc = a x up_bf.v CROSS(mag_corrected_local.v, qf->ahrs->north_vec.v, omc_mag); } else { omc_mag[X] = 0; omc_mag[Y] = 0; omc_mag[Z] = 0; } // get error correction gains depending on mode switch (qf->ahrs->internal_state) { case AHRS_UNLEVELED: kp = qf->kp * 10.0f; kp_mag = qf->kp_mag * 10.0f; ki = 0.5f * qf->ki; ki_mag = 0.5f * qf->ki_mag; convergence_update_count += 1; if( convergence_update_count > 2000 ) { convergence_update_count = 0; qf->ahrs->internal_state = AHRS_CONVERGING; print_util_dbg_print("End of AHRS attitude initialization.\r\n"); } break; case AHRS_CONVERGING: kp = qf->kp; kp_mag = qf->kp_mag; ki = qf->ki * 3.0f; ki_mag = qf->ki_mag * 3.0f; convergence_update_count += 1; if( convergence_update_count > 2000 ) { convergence_update_count = 0; qf->ahrs->internal_state = AHRS_READY; print_util_dbg_print("End of AHRS leveling.\r\n"); } break; case AHRS_READY: kp = qf->kp; kp_mag = qf->kp_mag; ki = qf->ki; ki_mag = qf->ki_mag; break; } // apply error correction with appropriate gains for accelerometer and compass for (uint8_t i = 0; i < 3; i++) { qtmp1.v[i] = 0.5f * (qf->imu->scaled_gyro.data[i] + kp * omc[i] + kp_mag * omc_mag[i]); } qtmp1.s = 0; // apply step rotation with corrections qed = quaternions_multiply(qf->ahrs->qe,qtmp1); // TODO: correct this formulas! qf->ahrs->qe.s = qf->ahrs->qe.s + qed.s * dt; qf->ahrs->qe.v[X] += qed.v[X] * dt; qf->ahrs->qe.v[Y] += qed.v[Y] * dt; qf->ahrs->qe.v[Z] += qed.v[Z] * dt; snorm = qf->ahrs->qe.s * qf->ahrs->qe.s + qf->ahrs->qe.v[X] * qf->ahrs->qe.v[X] + qf->ahrs->qe.v[Y] * qf->ahrs->qe.v[Y] + qf->ahrs->qe.v[Z] * qf->ahrs->qe.v[Z]; if (snorm < 0.0001f) { norm = 0.01f; } else { // approximate square root by running 2 iterations of newton method norm = maths_fast_sqrt(snorm); } qf->ahrs->qe.s /= norm; qf->ahrs->qe.v[X] /= norm; qf->ahrs->qe.v[Y] /= norm; qf->ahrs->qe.v[Z] /= norm; // bias estimate update qf->imu->calib_gyro.bias[X] += - dt * ki * omc[X] / qf->imu->calib_gyro.scale_factor[X]; qf->imu->calib_gyro.bias[Y] += - dt * ki * omc[Y] / qf->imu->calib_gyro.scale_factor[Y]; qf->imu->calib_gyro.bias[Z] += - dt * ki * omc[Z] / qf->imu->calib_gyro.scale_factor[Z]; qf->imu->calib_gyro.bias[X] += - dt * ki_mag * omc_mag[X] / qf->imu->calib_compass.scale_factor[X]; qf->imu->calib_gyro.bias[Y] += - dt * ki_mag * omc_mag[Y] / qf->imu->calib_compass.scale_factor[Y]; qf->imu->calib_gyro.bias[Z] += - dt * ki_mag * omc_mag[Z] / qf->imu->calib_compass.scale_factor[Z]; // set up-vector (bodyframe) in attitude qf->ahrs->up_vec.v[X] = up_bf.v[X]; qf->ahrs->up_vec.v[Y] = up_bf.v[Y]; qf->ahrs->up_vec.v[Z] = up_bf.v[Z]; // Update linear acceleration qf->ahrs->linear_acc[X] = 9.81f * (qf->imu->scaled_accelero.data[X] - qf->ahrs->up_vec.v[X]) ; // TODO: review this line! qf->ahrs->linear_acc[Y] = 9.81f * (qf->imu->scaled_accelero.data[Y] - qf->ahrs->up_vec.v[Y]) ; // TODO: review this line! qf->ahrs->linear_acc[Z] = 9.81f * (qf->imu->scaled_accelero.data[Z] - qf->ahrs->up_vec.v[Z]) ; // TODO: review this line! //update angular_speed. qf->ahrs->angular_speed[X] = qf->imu->scaled_gyro.data[X]; qf->ahrs->angular_speed[Y] = qf->imu->scaled_gyro.data[Y]; qf->ahrs->angular_speed[Z] = qf->imu->scaled_gyro.data[Z]; }