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);
}
}
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 forces_from_servos_diag_quad(simulation_model_t* sim)
{
float motor_command[4];
float rotor_lifts[4], rotor_drags[4], rotor_inertia[4];
float ldb;
quat_t wind_gf = {.s = 0, .v = {sim->vehicle_config.wind_x, sim->vehicle_config.wind_y, 0.0f}};
quat_t wind_bf = quaternions_global_to_local(sim->ahrs.qe, wind_gf);
float sqr_lateral_airspeed = SQR(sim->vel_bf[0] + wind_bf.v[0]) + SQR(sim->vel_bf[1] + wind_bf.v[1]);
float lateral_airspeed = sqrt(sqr_lateral_airspeed);
float old_rotor_speed;
for (int32_t i = 0; i < 4; i++)
{
motor_command[i] = (float)sim->mix->servos->servo[i].value - sim->vehicle_config.rotor_rpm_offset;
if (motor_command[i] < 0.0f)
{
motor_command[i] = 0;
}
// temporarily save old rotor speeds
old_rotor_speed = sim->rotorspeeds[i];
// estimate rotor speeds by low - pass filtering
//sim->rotorspeeds[i] = (sim->vehicle_config.rotor_lpf) * sim->rotorspeeds[i] + (1.0f - sim->vehicle_config.rotor_lpf) * (motor_command[i] * sim->vehicle_config.rotor_rpm_gain);
sim->rotorspeeds[i] = (motor_command[i] * sim->vehicle_config.rotor_rpm_gain);
// calculate torque created by rotor inertia
rotor_inertia[i] = (sim->rotorspeeds[i] - old_rotor_speed) / sim->dt * sim->vehicle_config.rotor_momentum;
ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, -sim->vel_bf[Z]);
//ldb = lift_drag_base(sim, sim->rotorspeeds[i], sqr_lateral_airspeed, 0.0f);
rotor_lifts[i] = ldb * sim->vehicle_config.rotor_cl;
rotor_drags[i] = ldb * sim->vehicle_config.rotor_cd;
}
float mpos_x = sim->vehicle_config.rotor_arm_length / 1.4142f;
float mpos_y = sim->vehicle_config.rotor_arm_length / 1.4142f;
// torque around x axis (roll)
sim->torques_bf[ROLL] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_rear_left] )
- (rotor_lifts[sim->mix->motor_front_right] + rotor_lifts[sim->mix->motor_rear_right] )) * mpos_y;;
// torque around y axis (pitch)
sim->torques_bf[PITCH] = ((rotor_lifts[sim->mix->motor_front_left] + rotor_lifts[sim->mix->motor_front_right] )
- (rotor_lifts[sim->mix->motor_rear_left] + rotor_lifts[sim->mix->motor_rear_right] ))* mpos_x;
sim->torques_bf[YAW] = (sim->mix->motor_front_left_dir * (10.0f * rotor_drags[sim->mix->motor_front_left] + rotor_inertia[sim->mix->motor_front_left])
+ sim->mix->motor_front_right_dir * (10.0f * rotor_drags[sim->mix->motor_front_right] + rotor_inertia[sim->mix->motor_front_right])
+ sim->mix->motor_rear_left_dir * (10.0f * rotor_drags[sim->mix->motor_rear_left] + rotor_inertia[sim->mix->motor_rear_left])
+ sim->mix->motor_rear_right_dir * (10.0f * rotor_drags[sim->mix->motor_rear_right] + rotor_inertia[sim->mix->motor_rear_right] ))* sim->vehicle_config.rotor_diameter;
sim->lin_forces_bf[X] = -(sim->vel_bf[X] - wind_bf.v[0]) * lateral_airspeed * sim->vehicle_config.vehicle_drag;
sim->lin_forces_bf[Y] = -(sim->vel_bf[Y] - wind_bf.v[1]) * lateral_airspeed * sim->vehicle_config.vehicle_drag;
sim->lin_forces_bf[Z] = -(rotor_lifts[sim->mix->motor_front_left]+ rotor_lifts[sim->mix->motor_front_right] +rotor_lifts[sim->mix->motor_rear_left] +rotor_lifts[sim->mix->motor_rear_right]);
}
//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------
bool simulation_init(simulation_model_t* sim, const simulation_config_t* sim_config, ahrs_t* ahrs, imu_t* imu, position_estimation_t* pos_est, barometer_t* pressure, gps_t* gps, sonar_t* sonar, state_t* state, bool* waypoint_set, const servo_mix_quadcotper_diag_t* mix)
{
bool init_success = true;
//Init dependencies
sim->vehicle_config = *sim_config;
sim->imu = imu;
sim->pos_est = pos_est;
sim->pressure = pressure;
sim->gps = gps;
sim->sonar = sonar;
sim->state = state;
sim->nav_plan_active = &state->nav_plan_active;
sim->mix = mix;
// set initial conditions to a given attitude_filter
sim->estimated_attitude = ahrs;
sim->ahrs = *ahrs;
print_util_dbg_print("Attitude:");
print_util_dbg_print_num(sim->ahrs.qe.s*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[0]*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[1]*100,10);
print_util_dbg_print(", ");
print_util_dbg_print_num(sim->ahrs.qe.v[2]*100,10);
print_util_dbg_print("\r\n");
for (int32_t i = 0; i < ROTORCOUNT; i++)
{
sim->rotorspeeds[i] = 0.0f;
}
sim->last_update = time_keeper_get_micros();
sim->dt = 0.01f;
simulation_reset_simulation(sim);
simulation_calib_set(sim);
print_util_dbg_print("[HIL SIMULATION] initialised.\r\n");
return init_success;
}
void stabilisation_wing_cascade_stabilise(stabilisation_wing_t* stabilisation_wing)
{
float rpyt_errors[4];
control_command_t input;
int32_t i;
float feedforward[4];
float nav_heading, current_heading, heading_error;
float gps_speed_global[3], gps_speed_semi_local[3];
float input_turn_rate;
float input_roll_angle;
aero_attitude_t attitude, attitude_yaw;
quat_t q_rot;
float airspeed_desired;
float clipping_factor;
// Get up vector in body frame
quat_t up = {0.0f, {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}};
quat_t up_vec = quaternions_global_to_local(stabilisation_wing->ahrs->qe,
up);
// set the controller input
input= *stabilisation_wing->controls;
switch (stabilisation_wing->controls->control_mode)
{
case VELOCITY_COMMAND_MODE:
// Get current attitude
attitude_yaw = coord_conventions_quat_to_aero(stabilisation_wing->ahrs->qe);
/////////////
// HEADING //
/////////////
// Compute the heading angle corresponding to the given input velocity vector (input from remote/vector field should be in semi-local frame).
nav_heading = heading_from_velocity_vector(input.tvel);
// Overwrite command if in remote
if(stabilisation_wing->controls->yaw_mode == YAW_RELATIVE)
{
nav_heading = maths_calc_smaller_angle(input.rpy[YAW] - attitude_yaw.rpy[YAW]);
}
// Compute current heading
gps_speed_global[X] = stabilisation_wing->gps->velocity_lf()[0];
gps_speed_global[Y] = stabilisation_wing->gps->velocity_lf()[1];
gps_speed_global[Z] = stabilisation_wing->pos_est->vel[Z];
// Transform global to semi-local
attitude_yaw.rpy[0] = 0.0f;
attitude_yaw.rpy[1] = 0.0f;
attitude_yaw.rpy[2] = -attitude_yaw.rpy[2];
q_rot = coord_conventions_quaternion_from_aero(attitude_yaw);
quaternions_rotate_vector(q_rot, gps_speed_global, gps_speed_semi_local);
current_heading = heading_from_velocity_vector(gps_speed_semi_local);
// Compute heading error
heading_error = maths_calc_smaller_angle(nav_heading - current_heading);
///////////////
// PID INPUT //
///////////////
// Vector field normalize vector in plane x-y to cruise_speed value ==> airspeed should be done only on the x-y composants
airspeed_desired = sqrtf(input.tvel[X]*input.tvel[X] + input.tvel[Y]*input.tvel[Y]);
// Compute errors
rpyt_errors[0] = heading_error; // Heading
rpyt_errors[1] = input.tvel[Z] - gps_speed_global[Z]; // Vertical speed
rpyt_errors[2] = 0.0f;
rpyt_errors[3] = airspeed_desired - stabilisation_wing->airspeed_analog->get_airspeed(); // Airspeed
// Compute the feedforward
feedforward[0] = 0.0f;
feedforward[1] = 0.0f;
feedforward[2] = 0.0f;
feedforward[3] = (airspeed_desired - 13.0f)/8.0f + 0.2f;
// run PID update on all velocity controllers
stabilisation_run_feedforward(&stabilisation_wing->stabiliser_stack.velocity_stabiliser, stabilisation_wing->ahrs->dt_s, rpyt_errors, feedforward);
////////////////
// PID OUTPUT //
////////////////
// Get turn rate command and transform it into a roll angle command for next layer
input_turn_rate = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.rpy[0];
// TODO: Fix this in case of bad airspeed readings...
clipping_factor = stabilisation_wing->max_roll_angle / (PI/2.0f);
if(clipping_factor == 0.0f)
{
input_roll_angle = 0.0f;
}
else
{
input_roll_angle = clipping_factor * atanf( (1.0f/clipping_factor) * (stabilisation_wing->airspeed_analog->get_airspeed() * input_turn_rate / 9.81f) );
}
// Set input for next layer
input = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output;
input.rpy[0] = input_roll_angle;
input.rpy[1] = - stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.rpy[1];
input.thrust = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.thrust;
// Overwrite the commands during different key phases (take-off and landing)
if(stabilisation_wing->navigation->internal_state_ == Navigation::NAV_TAKEOFF)
{
// Take-off: fixed 0 roll angle, fixed defined pitch angle and fixed defined constant thrust value.
input.rpy[0] = 0.0f;
input.rpy[1] = stabilisation_wing->take_off_pitch;
input.thrust = stabilisation_wing->take_off_thrust;
}
else if(stabilisation_wing->navigation->internal_state_ == Navigation::NAV_LANDING)
{
// Landing: Limit the roll computed by the velocity layer (navigation), shut down the motor and impose a little pitch down to assure gliding without stall.
if(input.rpy[0] > stabilisation_wing->landing_max_roll)
{
input.rpy[0] = stabilisation_wing->landing_max_roll;
}
else if(input.rpy[0] < -stabilisation_wing->landing_max_roll)
{
input.rpy[0] = -stabilisation_wing->landing_max_roll;
}
input.rpy[1] = stabilisation_wing->landing_pitch;
input.thrust = -1.0f;
}
// -- no break here - we want to run the lower level modes as well! --
case ATTITUDE_COMMAND_MODE:
// Add "a priori on the pitch" to fly horizontally and to compensate for roll angle
attitude = coord_conventions_quat_to_aero(stabilisation_wing->ahrs->qe);
input.rpy[1] += stabilisation_wing->pitch_angle_apriori; // Constant compensation for horizontal
if(maths_f_abs(attitude.rpy[ROLL]) < PI/2.0f) // Compensation for the roll bank angle
{
input.rpy[1] += stabilisation_wing->pitch_angle_apriori_gain * maths_f_abs(input.rpy[0]*input.rpy[0]*input.rpy[0]);
}
// run absolute attitude_filter controller
rpyt_errors[0]= sinf(input.rpy[0]) + up_vec.v[1]; // Roll
rpyt_errors[1]= sinf(input.rpy[1]) - up_vec.v[0]; // Pitch
rpyt_errors[2]= 0.0f; // Yaw
rpyt_errors[3]= input.thrust; // no feedback for thrust at this level
// run PID update on all attitude_filter controllers
stabilisation_run(&stabilisation_wing->stabiliser_stack.attitude_stabiliser, stabilisation_wing->ahrs->dt_s, rpyt_errors);
// use output of attitude_filter controller to set rate setpoints for rate controller
input = stabilisation_wing->stabiliser_stack.attitude_stabiliser.output;
// -- no break here - we want to run the lower level modes as well! --
case RATE_COMMAND_MODE: // this level is always run
// get rate measurements from IMU (filtered angular rates)
for (i=0; i<3; i++)
{
rpyt_errors[i]= input.rpy[i]- stabilisation_wing->ahrs->angular_speed[i];
}
rpyt_errors[3] = input.thrust ; // no feedback for thrust at this level
// run PID update on all rate controllers
stabilisation_run(&stabilisation_wing->stabiliser_stack.rate_stabiliser, stabilisation_wing->ahrs->dt_s, rpyt_errors);
}
stabilisation_wing->torque_command->xyz[0] = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.rpy[ROLL];
stabilisation_wing->torque_command->xyz[1] = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.rpy[PITCH];
stabilisation_wing->thrust_command->thrust = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.thrust;
}
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];
}
