void stabilisation_copter_send_outputs(stabilisation_copter_t* stabilisation_copter, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
aero_attitude_t attitude_yaw_inverse;
quat_t q_rot,qtmp;
attitude_yaw_inverse = coord_conventions_quat_to_aero(stabilisation_copter->ahrs->qe);
attitude_yaw_inverse.rpy[0] = 0.0f;
attitude_yaw_inverse.rpy[1] = 0.0f;
attitude_yaw_inverse.rpy[2] = attitude_yaw_inverse.rpy[2];
q_rot = coord_conventions_quaternion_from_aero(attitude_yaw_inverse);
qtmp=quaternions_create_from_vector(stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy);
quat_t rpy_local;
quaternions_rotate_vector(quaternions_inverse(q_rot), qtmp.v, rpy_local.v);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutVel",
time_keeper_get_micros(),
-rpy_local.v[X] * 1000,
rpy_local.v[Y] * 1000,
stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutAtt",
time_keeper_get_micros(),
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutRate",
time_keeper_get_micros(),
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
}
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 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];
}
void stabilisation_copter_cascade_stabilise(stabilisation_copter_t* stabilisation_copter)
{
float rpyt_errors[4];
control_command_t input;
int32_t i;
quat_t qtmp, q_rot;
aero_attitude_t attitude_yaw_inverse;
// set the controller input
input= *stabilisation_copter->controls;
switch (stabilisation_copter->controls->control_mode)
{
case VELOCITY_COMMAND_MODE:
attitude_yaw_inverse = coord_conventions_quat_to_aero(stabilisation_copter->ahrs->qe);
attitude_yaw_inverse.rpy[0] = 0.0f;
attitude_yaw_inverse.rpy[1] = 0.0f;
attitude_yaw_inverse.rpy[2] = attitude_yaw_inverse.rpy[2];
//qtmp=quaternions_create_from_vector(input.tvel);
//quat_t input_global = quaternions_local_to_global(stabilisation_copter->ahrs->qe, qtmp);
q_rot = coord_conventions_quaternion_from_aero(attitude_yaw_inverse);
quat_t input_global;
quaternions_rotate_vector(q_rot, input.tvel, input_global.v);
input.tvel[X] = input_global.v[X];
input.tvel[Y] = input_global.v[Y];
input.tvel[Z] = input_global.v[Z];
rpyt_errors[X] = input.tvel[X] - stabilisation_copter->pos_est->vel[X];
rpyt_errors[Y] = input.tvel[Y] - stabilisation_copter->pos_est->vel[Y];
rpyt_errors[3] = -(input.tvel[Z] - stabilisation_copter->pos_est->vel[Z]);
if (stabilisation_copter->controls->yaw_mode == YAW_COORDINATED)
{
float rel_heading_coordinated;
if ((maths_f_abs(stabilisation_copter->pos_est->vel_bf[X])<0.001f)&&(maths_f_abs(stabilisation_copter->pos_est->vel_bf[Y])<0.001f))
{
rel_heading_coordinated = 0.0f;
}
else
{
rel_heading_coordinated = atan2(stabilisation_copter->pos_est->vel_bf[Y], stabilisation_copter->pos_est->vel_bf[X]);
}
float w = 0.5f * (maths_sigmoid(vectors_norm(stabilisation_copter->pos_est->vel_bf)-stabilisation_copter->stabiliser_stack.yaw_coordination_velocity) + 1.0f);
input.rpy[YAW] = (1.0f - w) * input.rpy[YAW] + w * rel_heading_coordinated;
}
rpyt_errors[YAW]= input.rpy[YAW];
// run PID update on all velocity controllers
stabilisation_run(&stabilisation_copter->stabiliser_stack.velocity_stabiliser, stabilisation_copter->imu->dt, rpyt_errors);
//velocity_stabiliser.output.thrust = maths_f_min(velocity_stabiliser.output.thrust,stabilisation_param.controls->thrust);
stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.thrust += stabilisation_copter->thrust_hover_point;
stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.theading = input.theading;
input = stabilisation_copter->stabiliser_stack.velocity_stabiliser.output;
qtmp=quaternions_create_from_vector(stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy);
//quat_t rpy_local = quaternions_global_to_local(stabilisation_copter->ahrs->qe, qtmp);
quat_t rpy_local;
quaternions_rotate_vector(quaternions_inverse(q_rot), qtmp.v, rpy_local.v);
input.rpy[ROLL] = rpy_local.v[Y];
input.rpy[PITCH] = -rpy_local.v[X];
//input.thrust = stabilisation_copter->controls->tvel[Z];
// -- no break here - we want to run the lower level modes as well! --
case ATTITUDE_COMMAND_MODE:
// run absolute attitude_filter controller
rpyt_errors[0]= input.rpy[0] - ( - stabilisation_copter->ahrs->up_vec.v[1] );
rpyt_errors[1]= input.rpy[1] - stabilisation_copter->ahrs->up_vec.v[0];
if ((stabilisation_copter->controls->yaw_mode == YAW_ABSOLUTE) )
{
rpyt_errors[2] =maths_calc_smaller_angle(input.theading- stabilisation_copter->pos_est->local_position.heading);
}
else
{ // relative yaw
rpyt_errors[2]= input.rpy[2];
}
rpyt_errors[3]= input.thrust; // no feedback for thrust at this level
// run PID update on all attitude_filter controllers
stabilisation_run(&stabilisation_copter->stabiliser_stack.attitude_stabiliser, stabilisation_copter->imu->dt, rpyt_errors);
// use output of attitude_filter controller to set rate setpoints for rate controller
input = stabilisation_copter->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_copter->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_copter->stabiliser_stack.rate_stabiliser, stabilisation_copter->imu->dt, rpyt_errors);
}
// mix to servo outputs depending on configuration
if( stabilisation_copter->motor_layout == QUADCOPTER_MOTOR_LAYOUT_DIAG )
{
stabilisation_copter_mix_to_servos_diag_quad(&stabilisation_copter->stabiliser_stack.rate_stabiliser.output, stabilisation_copter->servos);
}
else if( stabilisation_copter->motor_layout == QUADCOPTER_MOTOR_LAYOUT_CROSS )
{
stabilisation_copter_mix_to_servos_cross_quad(&stabilisation_copter->stabiliser_stack.rate_stabiliser.output, stabilisation_copter->servos);
}
}
