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; }
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; }
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 ); }
void stabilisation_copter_position_hold(stabilisation_copter_t* stabilisation_copter, const control_command_t* input, const mavlink_waypoint_handler_t* waypoint_handler, const position_estimation_t* position_estimation) { aero_attitude_t attitude_yaw_inverse; quat_t q_rot; 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); float pos_error[4]; pos_error[X] = waypoint_handler->waypoint_hold_coordinates.pos[X] - position_estimation->local_position.pos[X]; pos_error[Y] = waypoint_handler->waypoint_hold_coordinates.pos[Y] - position_estimation->local_position.pos[Y]; pos_error[3] = -(waypoint_handler->waypoint_hold_coordinates.pos[Z] - position_estimation->local_position.pos[Z]); pos_error[YAW]= input->rpy[YAW]; // run PID update on all velocity controllers stabilisation_run(&stabilisation_copter->stabiliser_stack.position_stabiliser, stabilisation_copter->imu->dt, pos_error); float pid_output_global[3]; pid_output_global[0] = stabilisation_copter->stabiliser_stack.position_stabiliser.output.rpy[0]; pid_output_global[1] = stabilisation_copter->stabiliser_stack.position_stabiliser.output.rpy[1]; pid_output_global[2] = stabilisation_copter->stabiliser_stack.position_stabiliser.output.thrust + stabilisation_copter->thrust_hover_point; float pid_output_local[3]; quaternions_rotate_vector(q_rot, pid_output_global, pid_output_local); *stabilisation_copter->controls = *input; stabilisation_copter->controls->rpy[ROLL] = pid_output_local[Y]; stabilisation_copter->controls->rpy[PITCH] = -pid_output_local[X]; stabilisation_copter->controls->thrust = pid_output_local[2]; stabilisation_copter->controls->control_mode = ATTITUDE_COMMAND_MODE;//VELOCITY_COMMAND_MODE; }
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; }
bool Sonar_sim::update(void) { /** * H: scalar (altitude) * D: scalar (measure) * z: down vector ||z||=1 * u: sensor vector ||u||=1 * * With * Altitude = H.z * Distance = D.u * * We have * D = H / (u.z) */ bool success = true; // Update dynamic model success &= dynamic_model_.update(); // Get current attitude and position quat_t attitude = dynamic_model_.attitude(); local_position_t position = dynamic_model_.position_lf(); // Get orientation in global frame std::array<float, 3> orientation = orientation_bf(); float orient_bf[3] = {orientation[X], orientation[Y], orientation[Z]}; float orient_lf[3]; quaternions_rotate_vector(attitude, orient_bf, orient_lf); // Get u.z (u: sensor orientation, z: vertical down) // We keep only values with ratio > 0.707, ie. when the angle between // vertical and sensor is lower than 45 degrees float down[3] = {0.0f, 0.0f, 1.0f}; float ratio = vectors_scalar_product(orient_lf, down); if (ratio > 0.707) { float new_distance = - position[Z] / ratio; if (new_distance > config_.min_distance && new_distance < config_.max_distance) { float dt_s = (last_update_us_ - dynamic_model_.last_update_us()) / 1000000.0f; // Update velocity if (healthy_ && dt_s != 0.0f) { float new_velocity = (new_distance - distance_) / dt_s; //discard sonar new_velocity estimation if it seems too big if (maths_f_abs(new_velocity) > 20.0f) { new_velocity = 0.0f; } velocity_ = 0.5f * velocity_ + 0.5f * new_velocity; } else { velocity_ = 0.0f; } distance_ = new_distance; healthy_ = true; } else { // Update current distance even if not healthy if (new_distance < config_.min_distance) { distance_ = config_.min_distance; } else if(new_distance > config_.max_distance) { distance_ = config_.max_distance; } velocity_ = 0.0f; healthy_ = false; } } else { distance_ = config_.max_distance; velocity_ = 0.0f; healthy_ = false; } last_update_us_ = dynamic_model_.last_update_us(); return success; }
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); } }