/**
* \brief Vector field for attractor object
*
* \details Computes a 3D velocity vector attracting the MAV towards a 3D location
*
* \param pos_mav Current position of the MAV (input)
* \param pos_obj Position of the attractor (input)
* \param attractiveness Weight given to this object (input)
* \param vector Velocity command vector (output)
*/
static void vector_field_attractor(const float pos_mav[3], const float pos_obj[3], const float attractiveness, float vector[3])
{
float direction[3] = {0.0f, 0.0f, 0.0f}; // desired direction (unit vector)
float speed = 0.0f; // desired speed
/**
* Student code Here
*/
// Compute vector from MAV to goal
float mav_to_obj[3] = { pos_obj[X] - pos_mav[X],
pos_obj[Y] - pos_mav[Y],
pos_obj[Z] - pos_mav[Z]
};
// Compute norm of this vector
float dist_to_object = vectors_norm(mav_to_obj);
// Get desired direction
direction[X] = mav_to_obj[X] / dist_to_object;
direction[Y] = mav_to_obj[Y] / dist_to_object;
direction[Z] = mav_to_obj[Z] / dist_to_object;
// Compute desired speed
speed = attractiveness * dist_to_object;
/**
* End of Student code
*/
vector[X] = direction[X] * speed;
vector[Y] = direction[Y] * speed;
vector[Z] = direction[Z] * speed;
}
/**
* \brief Vector field for repulsive object
*
* \details Computes a 3D velocity vector pushing the MAV away from a 3D location
*
* \param pos_mav Current position of the MAV (input)
* \param pos_obj Position of the repulsor (input)
* \param repulsiveness Weight given to this object (input)
* \param max_range Range of action (in m)
* \param safety_radius Minimum distance the object can be approached (in m)
* \param vector Velocity command vector (output)
*/
static void vector_field_repulsor_sphere(const float pos_mav[3], const float pos_obj[3], const float repulsiveness, const float max_range, const float safety_radius, float vector[3])
{
float direction[3] = {0.0f, 0.0f, 0.0f}; // desired direction (unit vector)
float speed = 0.0f; // desired speed
/**
* Student code Here
*/
// Compute vector from MAV to goal
float mav_to_obj[3] = { pos_obj[X] - pos_mav[X],
pos_obj[Y] - pos_mav[Y],
pos_obj[Z] - pos_mav[Z]
};
// Compute norm of this vector
float dist_to_object = vectors_norm(mav_to_obj);
// Get desired direction
direction[X] = mav_to_obj[X] / dist_to_object;
direction[Y] = mav_to_obj[Y] / dist_to_object;
direction[Z] = mav_to_obj[Z] / dist_to_object;
// Compute desired speed
if (dist_to_object <= safety_radius)
{
speed = 0.0f;
}
else if (dist_to_object > max_range)
{
speed = 0.0f;
}
else
{
speed = - repulsiveness / (dist_to_object - safety_radius) * (max_range - dist_to_object);
}
/**
* End of Student code
*/
vector[X] = direction[X] * speed;
vector[Y] = direction[Y] * speed;
vector[Z] = direction[Z] * speed;
}
bool vector_field_waypoint_update(vector_field_waypoint_t* vector_field)
{
float tmp_vector[3];
float pos_obj[3];
// Re-init velocity command
vector_field->velocity_command->mode = VELOCITY_COMMAND_MODE_LOCAL;
vector_field->velocity_command->xyz[X] = 0.0f;
vector_field->velocity_command->xyz[Y] = 0.0f;
vector_field->velocity_command->xyz[Z] = 0.0f;
// Compute vector field for floor avoidance
vector_field_floor(vector_field->pos_est->local_position.pos,
20,
tmp_vector);
// Add floor vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
// Go through waypoint list
for (uint16_t i = 0; i < vector_field->waypoint_handler->waypoint_count(); ++i)
{
// Get waypoint in NED coordinates
Mavlink_waypoint_handler::waypoint_struct_t waypoint = convert_waypoint_to_local_ned(&vector_field->waypoint_handler->waypoint_list[i],
&vector_field->pos_est->local_position.origin);
// Get object position
pos_obj[X] = waypoint.x;
pos_obj[Y] = waypoint.y;
pos_obj[Z] = waypoint.z;
switch (waypoint.command)
{
// Attractive object
case 22:
vector_field_attractor(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // attractiveness
tmp_vector);
break;
// Cylindrical repulsive object
case 17:
vector_field_repulsor_cylinder(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // repulsiveness
waypoint.param2, // max_range
waypoint.param3, // safety_radius
tmp_vector);
break;
// Spherical repulsive object
case 18:
vector_field_repulsor_sphere(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // repulsiveness
waypoint.param2, // max_range
waypoint.param3, // safety_radius
tmp_vector);
break;
// Circular waypoint
case 16:
vector_field_circular_waypoint(vector_field->pos_est->local_position.pos,
pos_obj,
waypoint.param1, // attractiveness
waypoint.param2, // cruise_speed
waypoint.param3, // radius
tmp_vector);
break;
// Unknown object
default:
tmp_vector[X] = 0.0f;
tmp_vector[Y] = 0.0f;
tmp_vector[Z] = 0.0f;
break;
}
// Add vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
}
// Limit velocity
float vel_norm = vectors_norm(vector_field->velocity_command->xyz);
if (vel_norm > 5.0f)
{
vector_field->velocity_command->xyz[X] = 5.0f * vector_field->velocity_command->xyz[X] / vel_norm;
vector_field->velocity_command->xyz[Y] = 5.0f * vector_field->velocity_command->xyz[Y] / vel_norm;
vector_field->velocity_command->xyz[Z] = 5.0f * vector_field->velocity_command->xyz[Z] / vel_norm;
}
return true;
}
bool vector_field_waypoint_update(vector_field_waypoint_t* vector_field)
{
float tmp_vector[3];
float pos_obj[3];
// Re-init velocity command
vector_field->velocity_command->xyz[X] = 0.0f;
vector_field->velocity_command->xyz[Y] = 0.0f;
vector_field->velocity_command->xyz[Z] = 0.0f;
// Compute vector field for floor avoidance
vector_field_floor(vector_field->ins->position_lf().data(),
20,
tmp_vector);
// Add floor vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
// Go through waypoint list
for (uint16_t i = 0; i < vector_field->waypoint_handler->waypoint_count(); ++i)
{
const Waypoint& waypoint = vector_field->waypoint_handler->waypoint_from_index(i);
local_position_t local_wpt = waypoint.local_pos();
// Get object position
pos_obj[X] = local_wpt[X];
pos_obj[Y] = local_wpt[Y];
pos_obj[Z] = local_wpt[Z];
switch (waypoint.command())
{
// Attractive object
case 22:
vector_field_attractor(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // attractiveness
tmp_vector);
break;
// Cylindrical repulsive object
case 17:
vector_field_repulsor_cylinder(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // repulsiveness
waypoint.param2(), // max_range
waypoint.param3(), // safety_radius
tmp_vector);
break;
// Spherical repulsive object
case 18:
vector_field_repulsor_sphere(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // repulsiveness
waypoint.param2(), // max_range
waypoint.param3(), // safety_radius
tmp_vector);
break;
// Circular waypoint
case 16:
vector_field_circular_waypoint(vector_field->ins->position_lf().data(),
pos_obj,
waypoint.param1(), // attractiveness
waypoint.param2(), // cruise_speed
waypoint.param3(), // radius
tmp_vector);
break;
// Unknown object
default:
tmp_vector[X] = 0.0f;
tmp_vector[Y] = 0.0f;
tmp_vector[Z] = 0.0f;
break;
}
// Add vector field to the velocity command
vector_field->velocity_command->xyz[X] += tmp_vector[X];
vector_field->velocity_command->xyz[Y] += tmp_vector[Y];
vector_field->velocity_command->xyz[Z] += tmp_vector[Z];
}
// Limit velocity
float vel_norm = vectors_norm(vector_field->velocity_command->xyz.data());
if (vel_norm > 5.0f)
{
vector_field->velocity_command->xyz[X] = 5.0f * vector_field->velocity_command->xyz[X] / vel_norm;
vector_field->velocity_command->xyz[Y] = 5.0f * vector_field->velocity_command->xyz[Y] / vel_norm;
vector_field->velocity_command->xyz[Z] = 5.0f * vector_field->velocity_command->xyz[Z] / vel_norm;
}
return true;
}
void velocity_controller_copter_update(velocity_controller_copter_t* controller)
{
float velocity_command_global[3];
float errors[3];
float thrust_vector[3];
float thrust_norm;
float thrust_dir[3];
// Get the command velocity in global frame
switch( controller->velocity_command->mode )
{
case VELOCITY_COMMAND_MODE_LOCAL:
get_velocity_command_from_local_to_global( controller,
velocity_command_global );
break;
case VELOCITY_COMMAND_MODE_SEMI_LOCAL:
get_velocity_command_from_semilocal_to_global( controller,
velocity_command_global );
break;
case VELOCITY_COMMAND_MODE_GLOBAL:
velocity_command_global[X] = controller->velocity_command->xyz[X];
velocity_command_global[Y] = controller->velocity_command->xyz[Y];
velocity_command_global[Z] = controller->velocity_command->xyz[Z];
break;
default:
velocity_command_global[X] = 0.0f;
velocity_command_global[Y] = 0.0f;
velocity_command_global[Z] = 0.0f;
break;
}
// Compute errors
errors[X] = velocity_command_global[X] - controller->pos_est->vel[X];
errors[Y] = velocity_command_global[Y] - controller->pos_est->vel[Y];
errors[Z] = velocity_command_global[Z] - controller->pos_est->vel[Z]; // WARNING: it was multiplied by (-1) in stabilisation_copter.c
// Update PID
thrust_vector[X] = pid_controller_update_dt( &controller->pid[X], errors[X], controller->ahrs->dt ); // should be multiplied by mass
thrust_vector[Y] = pid_controller_update_dt( &controller->pid[Y], errors[Y], controller->ahrs->dt ); // should be multiplied by mass
// thrust_vector[Z] = - GRAVITY + pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt ); // should be multiplied by mass
thrust_vector[Z] = pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt ); // should be multiplied by mass
// Compute the norm of the thrust that should be applied
thrust_norm = vectors_norm(thrust_vector);
// Compute the direction in which thrust should be apply
thrust_dir[X] = thrust_vector[X] / thrust_norm;
thrust_dir[Y] = thrust_vector[Y] / thrust_norm;
thrust_dir[Z] = thrust_vector[Z] / thrust_norm;
// Map thrust dir to attitude
controller->attitude_command->mode = ATTITUDE_COMMAND_MODE_RPY;
controller->attitude_command->rpy[ROLL] = maths_clip(thrust_vector[Y], 1);
controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
controller->attitude_command->rpy[YAW] = 0.0f;
// Map PID output to thrust
// float max_thrust = 30.0f; // 10 Newton max thrust
// controller->thrust_command->thrust = maths_clip(thrust_norm/max_thrust, 1.0f) * 2.0f - 1;
// controller->thrust_command->thrust = maths_clip(thrust_norm, 1.0f) * 2.0f - 1;
// controller->thrust_command->thrust = - 1.0 + 2 * thrust_norm/max_thrust;
// // Map PID output to attitude
// controller->attitude_command->mode = ATTITUDE_COMMAND_MODE_RPY;
// controller->attitude_command->rpy[ROLL] = maths_clip(thrust_vector[Y], 1);
// controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
// controller->attitude_command->rpy[YAW] = 0.0f;
// Map PID output to thrust
controller->thrust_command->thrust = controller->thrust_hover_point - thrust_vector[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);
}
}
