bool attitude_error_estimator_update(attitude_error_estimator_t* estimator)
{
quat_t quat_attitude = estimator->ahrs->qe;
quat_t quat_ref = estimator->quat_ref;
quat_t quat_error;
// Compute quaternioin error in global frame
quat_error = quaternions_multiply(quat_ref, quaternions_inverse(quat_attitude));
// Express error in local coordinates
quat_error = quaternions_multiply( quaternions_inverse(quat_attitude),
quaternions_multiply(quat_error,
quat_attitude) );
// Find shortest rotation
if (quat_error.s < 0)
{
quat_error = quaternions_inverse(quat_error);
}
// Approximate roll pitch and yaw errors with quat_error vector part
estimator->rpy_errors[0] = 2 * quat_error.v[0];
estimator->rpy_errors[1] = 2 * quat_error.v[1];
estimator->rpy_errors[2] = 2 * quat_error.v[2];
return true;
}
bool Magnetometer_sim::update(void)
{
bool success = true;
// Update dynamic model
success &= dynamic_model_.update();
// Field pointing 62 degrees down to the north (NED)
const float mag_field_lf[3] = { 0.46947156f, 0.0f, 0.88294759f };
float mag_field_bf[3];
// Get current attitude
quat_t attitude = dynamic_model_.attitude();
// Get magnetic field in body frame
quaternions_rotate_vector(quaternions_inverse(attitude), mag_field_lf, mag_field_bf);
// quaternions_rotate_vector( attitude, mag_field_lf, mag_field_bf);
// Save in member array
mag_field_[X] = mag_field_bf[X];
mag_field_[Y] = mag_field_bf[Y];
mag_field_[Z] = mag_field_bf[Z];
return success;
}
bool Flow_sim::update(void)
{
raytracing::Ray ray_tmp;
raytracing::Intersection inter_tmp;
raytracing::Object* obj_tmp = NULL;
Mat<2,1> of_tmp;
// Get current velocity
quat_t att = dynamic_model_.attitude();
float vel_lf[3] = {dynamic_model_.velocity_lf()[0], dynamic_model_.velocity_lf()[1], dynamic_model_.velocity_lf()[2]};
float vel_bf[3];
quaternions_rotate_vector(att, vel_lf, vel_bf);
Mat<3,1> vel;
vel[0] = vel_bf[0];
vel[1] = vel_bf[1];
vel[2] = vel_bf[2];
for (uint32_t i = 0; i < of_count; i++)
{
// Translate ray
local_position_t pos_lf = dynamic_model_.position_lf();
ray_tmp.set_origin(Vector3f{pos_lf.pos[0], pos_lf.pos[1], pos_lf.pos[2]});
// Rotate ray
float orient_bf[3] = {rays_[i].direction()[0], rays_[i].direction()[1], rays_[i].direction()[2]};
float orient_lf[3];
quaternions_rotate_vector( quaternions_inverse(att), orient_bf, orient_lf);
ray_tmp.set_direction(Vector3f{orient_lf[0], orient_lf[1], orient_lf[2]});
// Test intersection
float proximity;
if (world_.intersect(ray_tmp, inter_tmp, obj_tmp))
{
proximity = 1.0f / inter_tmp.distance();
}
else
{
proximity = 0.0f;
}
// Compute optic flow
// of_tmp = jacob_[i] % (vel * proximity);
// of.x[i] = of_tmp[0];
// of.y[i] = of_tmp[1];
of.x[i] = (vel[0] * proximity) * quick_trig_sin(of_loc.x[i]);
of.y[i] = 0.0f;
}
return true;
}
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 get_velocity_command_from_semilocal_to_global(const velocity_controller_copter_t* controller, float command[3])
{
aero_attitude_t semilocal_frame_rotation;
quat_t q_semilocal;
// Get current heading
float heading = coord_conventions_quat_to_aero(controller->ahrs->qe).rpy[YAW];
semilocal_frame_rotation.rpy[ROLL] = 0.0f;
semilocal_frame_rotation.rpy[PITCH] = 0.0f;
semilocal_frame_rotation.rpy[YAW] = heading;
// Get rotation quaternion from semilocal frame to global frame
q_semilocal = coord_conventions_quaternion_from_aero( semilocal_frame_rotation );
// Rotate command from semilocal to global
quaternions_rotate_vector( quaternions_inverse( q_semilocal ), // TODO: Check why this is not "quaternions_inverse( q_semilocal )" here
// quaternions_rotate_vector( q_semilocal,
controller->velocity_command->xyz,
command );
}
static void get_velocity_command_from_local_to_global(const velocity_controller_copter_t* controller, float command[3])
{
quaternions_rotate_vector( quaternions_inverse( controller->ahrs->qe ),
controller->velocity_command->xyz,
command);
}
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);
}
}
