// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination // this function updates the _yaw_error_cd value void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed) { // record system time of call last_steer_to_wp_ms = AP_HAL::millis(); // Calculate the required turn of the wheels // negative error = left turn // positive error = right turn rover.nav_controller->set_reverse(reversed); rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius); float desired_lat_accel = rover.nav_controller->lateral_acceleration(); float desired_heading = rover.nav_controller->target_bearing_cd(); if (reversed) { _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor + 18000); } else { _yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor); } if (rover.use_pivot_steering(_yaw_error_cd)) { // for pivot turns use heading controller calc_steering_to_heading(desired_heading, reversed); } else { // call lateral acceleration to steering controller calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); } }
void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed) { // Pitch angle error in centidegrees // Positive error means the target is above current pitch // Negative error means the target is below current pitch float ahrs_pitch = ahrs.pitch_sensor; int32_t ef_pitch_angle_error = pitch - ahrs_pitch; // Yaw angle error in centidegrees // Positive error means the target is right of current yaw // Negative error means the target is left of current yaw int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd); if (direction_reversed) { if (ef_yaw_angle_error > 0) { ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000; } else { ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd); } } // earth frame to body frame angle error conversion float bf_pitch_err; float bf_yaw_err; convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err); nav_status.angle_error_pitch = bf_pitch_err; nav_status.angle_error_yaw = bf_yaw_err; }
void ModeLoiter::update() { // get distance (in meters) to destination _distance_to_destination = get_distance(rover.current_loc, _destination); // if within waypoint radius slew desired speed towards zero and use existing desired heading if (_distance_to_destination <= g.waypoint_radius) { _desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt); _yaw_error_cd = 0.0f; } else { // P controller with hard-coded gain to convert distance to desired speed // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise); // calculate bearing to destination _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); // if destination is behind vehicle, reverse towards it if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) { _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _desired_speed = -_desired_speed; } // reduce desired speed if yaw_error is large // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50% float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f; _desired_speed *= yaw_error_ratio; } // run steering and throttle controllers calc_steering_to_heading(_desired_yaw_cd); calc_throttle(_desired_speed, false, true); }
// guided_angle_control_run - runs the guided angle controller // called from guided_run void Copter::guided_angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.set_yaw_target_to_current_heading(); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); attitude_control.set_throttle_out(0.0f,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(0.0f); return; } // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; pitch_in *= ratio; } // wrap yaw request float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds); // constrain climb rate float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up()); // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { roll_in = 0.0f; pitch_in = 0.0f; climb_rate_cms = 0.0f; yaw_rate_in = 0.0f; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller if (guided_angle_state.use_yaw_rate) { attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain()); } else { attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain()); } // call position controller pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); pos_control.update_z_controller(); }
/** update the yaw continuous rotation servo */ void Tracker::update_yaw_cr_servo(float yaw) { int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); float yaw_cd = wrap_180_cd(yaw*100.0f); float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd); g.pidYaw2Srv.set_input_filter_all(err_cd); channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid()); }
// guided_angle_control_run - runs the guided angle controller // called from guided_run void Copter::ModeGuided::angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { #if FRAME_CONFIG == HELI_FRAME attitude_control->set_yaw_target_to_current_heading(); #endif zero_throttle_and_relax_ac(); pos_control->relax_alt_hold_controllers(0.0f); return; } // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; pitch_in *= ratio; } // wrap yaw request float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds); // constrain climb rate float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up()); // get avoidance adjusted climb rate climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) { roll_in = 0.0f; pitch_in = 0.0f; climb_rate_cms = 0.0f; yaw_rate_in = 0.0f; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller if (guided_angle_state.use_yaw_rate) { attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in); } else { attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); } // call position controller pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); pos_control->update_z_controller(); }
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max) { // calculate angle error with maximum of +- max angle overshoot angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); angle_ef_error.x = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max); // update roll angle target to be within max angle overshoot of our roll angle _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor; // increment the roll angle target _angle_ef_target.x += roll_rate_ef * _dt; _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); }
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error) { // calculate angle error with maximum of +- max angle overshoot angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor); angle_ef_error.x = constrain_float(angle_ef_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); // update roll angle target to be within max angle overshoot of our roll angle _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor; // increment the roll angle target _angle_ef_target.x += roll_rate_ef * _dt; _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); }
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max) { // calculate angle error with maximum of +- max angle overshoot angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);//把误差转化到180度之内,此误差为上一个目标点与实际角度的误差 angle_ef_error.x = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max); // update roll angle target to be within max angle overshoot of our roll angle误差加传感器的值为目标值 _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;//修正当前目标点(猜测在修正传感器误差) // increment the roll angle target,把roll角目标更新到下一时刻 _angle_ef_target.x += roll_rate_ef * _dt;//更新下一个目标点 _angle_ef_target.x = wrap_180_cd(_angle_ef_target.x); }
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error) { // calculate angle error with maximum of +- max angle overshoot // To-Do: should we do something better as we cross 90 degrees? angle_ef_error.y = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor); angle_ef_error.y = constrain_float(angle_ef_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); // update pitch angle target to be within max angle overshoot of our pitch angle _angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor; // increment the pitch angle target _angle_ef_target.y += pitch_rate_ef * _dt; _angle_ef_target.y = wrap_180_cd(_angle_ef_target.y); }
// return value is true if taking the long road to the target, false if normal, shortest direction should be used bool Tracker::get_ef_yaw_direction() { // calculating distances from current pitch/yaw to lower and upper limits in centi-degrees float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get(); float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get(); float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get(); float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get(); // distances to earthframe angle limits in centi-degrees float ef_yaw_limit_lower = yaw_angle_limit_lower; float ef_yaw_limit_upper = yaw_angle_limit_upper; float ef_pitch_limit_lower = pitch_angle_limit_lower; float ef_pitch_limit_upper = pitch_angle_limit_upper; convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower); convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper); float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor); float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current); // calculate angle error to target in both directions (i.e. moving up/right or lower/left) float yaw_angle_error_upper; float yaw_angle_error_lower; if (ef_yaw_angle_error >= 0) { yaw_angle_error_upper = ef_yaw_angle_error; yaw_angle_error_lower = ef_yaw_angle_error - 36000; } else { yaw_angle_error_upper = 36000 + ef_yaw_angle_error; yaw_angle_error_lower = ef_yaw_angle_error; } // checks that the vehicle is outside the tracker's range if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) { // if the tracker is trying to move clockwise to reach the vehicle, // but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) { return true; } // if the tracker is trying to move counter-clockwise to reach the vehicle, // but the tracker could get closer to the vehicle by moving then set direction_reversed to true if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) { return true; } } // if we get this far we can use the regular, shortest path to the target return false; }
// set guided mode angle target void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) { // check we are in velocity control mode if (guided_mode != Guided_Angle) { angle_control_start(); } // convert quaternion to euler angles q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f; guided_angle_state.use_yaw_rate = use_yaw_rate; guided_angle_state.climb_rate_cms = climb_rate_cms; guided_angle_state.update_time_ms = millis(); // interpret positive climb rate as triggering take-off if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) { copter.set_auto_armed(true); } // log target copter.Log_Write_GuidedTarget(guided_mode, Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms)); }
// set desired location void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { // set origin to last destination if waypoint controller active if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) { _origin = _destination; } else { // otherwise use reasonable stopping point calc_stopping_location(_origin); } _destination = destination; // initialise distance _distance_to_destination = get_distance(_origin, _destination); _reached_destination = false; // set final desired speed _desired_speed_final = 0.0f; if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) { const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination); const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd); if (is_zero(turn_angle_cd)) { // if not turning can continue at full speed _desired_speed_final = _desired_speed; } else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) { // pivoting so we will stop _desired_speed_final = 0.0f; } else { // calculate maximum speed that keeps overshoot within bounds const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f)); _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m)); } } }
/* update the total angle we have covered in a loiter. Used to support commands to do N circles of loiter */ void Plane::loiter_angle_update(void) { static const int32_t lap_check_interval_cd = 3*36000; const int32_t target_bearing_cd = nav_controller->target_bearing_cd(); int32_t loiter_delta_cd; if (loiter.sum_cd == 0 && !reached_loiter_target()) { // we don't start summing until we are doing the real loiter loiter_delta_cd = 0; } else if (loiter.sum_cd == 0) { // use 1 cd for initial delta loiter_delta_cd = 1; loiter.start_lap_alt_cm = current_loc.alt; loiter.next_sum_lap_cd = lap_check_interval_cd; } else { loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd; } loiter.old_target_bearing_cd = target_bearing_cd; loiter_delta_cd = wrap_180_cd(loiter_delta_cd); loiter.sum_cd += loiter_delta_cd * loiter.direction; if (labs(current_loc.alt - next_WP_loc.alt) < 500) { loiter.reached_target_alt = true; loiter.unable_to_acheive_target_alt = false; loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd; } else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) { // check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500; loiter.start_lap_alt_cm = current_loc.alt; loiter.next_sum_lap_cd += lap_check_interval_cd; } }
static void test_wrap_cd(void) { for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) { int32_t r = wrap_180_cd(wrap_180_tests[i].v); if (r != wrap_180_tests[i].wv) { hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n", (long)wrap_180_tests[i].v, (long)wrap_180_tests[i].wv, (long)r); } } for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) { int32_t r = wrap_360_cd(wrap_360_tests[i].v); if (r != wrap_360_tests[i].wv) { hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n", (long)wrap_360_tests[i].v, (long)wrap_360_tests[i].wv, (long)r); } } for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) { float r = wrap_PI(wrap_PI_tests[i].v); if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) { hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n", wrap_PI_tests[i].v, wrap_PI_tests[i].wv, r); } } hal.console->printf("wrap_cd tests done\n"); }
void Rover::calc_lateral_acceleration() { switch (control_mode) { case AUTO: // If we have reached the waypoint previously navigate // back to it from our current position if (previously_reached_wp && (loiter_duration > 0)) { nav_controller->update_waypoint(current_loc, next_WP); } else { nav_controller->update_waypoint(prev_WP, next_WP); } break; case RTL: case GUIDED: case STEERING: nav_controller->update_waypoint(current_loc, next_WP); break; default: return; } // Calculate the required turn of the wheels // negative error = left turn // positive error = right turn lateral_acceleration = nav_controller->lateral_acceleration(); if (use_pivot_steering()) { const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100; if (bearing_error > 0) { lateral_acceleration = g.turn_max_g * GRAVITY_MSS; } else { lateral_acceleration = -g.turn_max_g * GRAVITY_MSS; } } }
// set desired location void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { // record targets _origin = rover.current_loc; _destination = destination; // initialise distance _distance_to_destination = get_distance(_origin, _destination); _reached_destination = false; // set final desired speed _desired_speed_final = 0.0f; if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) { // if not turning can continue at full speed if (is_zero(next_leg_bearing_cd)) { _desired_speed_final = _desired_speed; } else { // calculate maximum speed that keeps overshoot within bounds const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination); const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd); const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f)); _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m)); } } }
/* Wrap AHRS yaw sensor if in reverse - centi-degress */ float AP_L1_Control::get_yaw_sensor() { if (_reverse) { return wrap_180_cd(18000 + _ahrs.yaw_sensor); } return _ahrs.yaw_sensor; }
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z; // get final angle, 1 = Relative, 0 = Absolute if (relative_angle == 0) { // absolute angle yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed if (is_zero(turn_rate_dps)) { // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; }else{ int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec } // set yaw mode set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise }
// return true if we should use the compass for yaw correction bool AP_AHRS_DCM::use_compass(void) { if (!_compass || !_compass->use_for_yaw()) { // no compass available return false; } if (!_flags.fly_forward || !have_gps()) { // we don't have any alterative to the compass return true; } if (_gps.ground_speed() < GPS_SPEED_MIN) { // we are not going fast enough to use the GPS return true; } // if the current yaw differs from the GPS yaw by more than 45 // degrees and the estimated wind speed is less than 80% of the // ground speed, then switch to GPS navigation. This will help // prevent flyaways with very bad compass offsets int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd())); if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) { if (AP_HAL::millis() - _last_consistent_heading > 2000) { // start using the GPS for heading if the compass has been // inconsistent with the GPS for 2 seconds return false; } } else { _last_consistent_heading = AP_HAL::millis(); } // use the compass return true; }
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) { if (_inverted_flight) { euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000); } AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw); }
// calculate steering output to drive along line from origin to destination waypoint // relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated void AR_WPNav::update_steering(const Location& current_loc, float current_speed) { // calculate yaw error for determining if vehicle should pivot towards waypoint float yaw_cd = _reversed ? wrap_360_cd(_wp_bearing_cd + 18000) : _wp_bearing_cd; float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor); // calculate desired turn rate and update desired heading if (use_pivot_steering(yaw_error_cd)) { _cross_track_error = 0.0f; _desired_heading_cd = yaw_cd; _desired_lat_accel = 0.0f; _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate)); } else { // run L1 controller _nav_controller.set_reverse(_reversed); _nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius); // retrieve lateral acceleration, heading back towards line and crosstrack error _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss); _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd()); if (_reversed) { _desired_lat_accel *= -1.0f; _desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000); } _cross_track_error = _nav_controller.crosstrack_error(); _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed); } }
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // To-Do: re-initialise wpnav targets return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { // check if heading is within 2 degrees of heading when vehicle was armed if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { rtl_state_complete = true; } } else { // we have loitered long enough rtl_state_complete = true; } } }
/* verify a LOITER_TO_ALT command. This involves checking we have reached both the desired altitude and desired heading. The desired altitude only needs to be reached once. */ bool Plane::verify_loiter_to_alt() { update_loiter(); //has target altitude been reached? if (condition_value != 0) { if (labs(condition_value - current_loc.alt) < 500) { //Only have to reach the altitude once -- that's why I need //this global condition variable. // //This is in case of altitude oscillation when still trying //to reach the target heading. condition_value = 0; } else { return false; } } //has target heading been reached? if (condition_value2 != 0) { //Get the lat/lon of next Nav waypoint after this one: AP_Mission::Mission_Command next_nav_cmd; if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd)) { //no next waypoint to shoot for -- go ahead and break out of loiter return true; } // Bearing in radians int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); // get current heading. We should probably be using the ground // course instead to improve the accuracy in wind int32_t heading_cd = ahrs.yaw_sensor; int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); /* Check to see if the the plane is heading toward the land waypoint We use 10 degrees of slop so that we can handle 100 degrees/second of yaw */ if (abs(heading_err_cd) <= 1000) { //Want to head in a straight line from _here_ to the next waypoint. //DON'T want to head in a line from the center of the loiter to //the next waypoint. //Therefore: mark the "last" (next_wp_loc is about to be updated) //wp lat/lon as the current location. next_WP_loc = current_loc; return true; } else { return false; } } return true; }
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed) { // handle initialisation if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) { _guided_mode = ModeGuided::Guided_HeadingAndSpeed; _desired_yaw_cd = ahrs.yaw_sensor; } set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed); }
/* work out if we are going to use pivot steering */ bool Rover::use_pivot_steering(void) { if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) { const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100; if (abs(bearing_error) > g.pivot_turn_angle) { return true; } } return false; }
// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint // relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members // have been updated: _wp_bearing_cd, _cross_track_error, _distance_to_destination void AR_WPNav::update_desired_speed(float dt) { // reduce speed to zero during pivot turns if (_pivot_active) { // decelerate to zero _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); return; } // accelerate desired speed towards max float des_speed_lim = _atc.get_desired_speed_accel_limited(_reversed ? -_desired_speed : _desired_speed, dt); // reduce speed to limit overshoot from line between origin and destination // calculate number of degrees vehicle must turn to face waypoint float ahrs_yaw_sensor = AP::ahrs().yaw_sensor; const float heading_cd = is_negative(des_speed_lim) ? wrap_180_cd(ahrs_yaw_sensor + 18000) : ahrs_yaw_sensor; const float wp_yaw_diff_cd = wrap_180_cd(_wp_bearing_cd - heading_cd); const float turn_angle_rad = fabsf(radians(wp_yaw_diff_cd * 0.01f)); // calculate distance from vehicle to line + wp_overshoot const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd); const float dist_from_line = fabsf(_cross_track_error); const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error); const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line; // calculate radius of circle that touches vehicle's current position and heading and target position and heading float radius_m = 999.0f; const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad)); if (!is_zero(radius_calc_denom)) { radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom; } // calculate and limit speed to allow vehicle to stay on circle const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m)); des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max); // limit speed based on distance to waypoint and max acceleration/deceleration if (is_positive(_distance_to_destination) && is_positive(_atc.get_decel_max())) { const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final)); des_speed_lim = constrain_float(des_speed_lim, -dist_speed_max, dist_speed_max); } _desired_speed_limited = des_speed_lim; }
bool Plane::verify_loiter_heading(bool init) { if (quadplane.in_vtol_auto()) { // skip heading verify if in VTOL auto return true; } //Get the lat/lon of next Nav waypoint after this one: AP_Mission::Mission_Command next_nav_cmd; if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd)) { //no next waypoint to shoot for -- go ahead and break out of loiter return true; } if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) { /* Whenever next waypoint is within the loiter radius, maintaining loiter would prevent us from ever pointing toward the next waypoint. Hence break out of loiter immediately */ return true; } // Bearing in degrees int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location); // get current heading. int32_t heading_cd = gps.ground_course_cd(); int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); if (init) { loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd); loiter.sum_cd = 0; } /* Check to see if the the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg) of margin so that we can handle 200 degrees/second of yaw. Allow turn count to stop it too to ensure we don't loop around forever in case high winds are forcing us beyond 200 deg/sec at this particular moment. */ if (labs(heading_err_cd) <= 1000 || loiter.sum_cd > loiter.total_cd) { // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location if (next_WP_loc.flags.loiter_xtrack) { next_WP_loc = current_loc; } return true; } return false; }
// verify_yaw - return true if we have reached the desired heading bool Sub::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading return (fabsf(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200); }
void ModeLoiter::update() { // get distance (in meters) to destination _distance_to_destination = get_distance(rover.current_loc, _destination); // if within waypoint radius slew desired speed towards zero and use existing desired heading if (_distance_to_destination <= g.waypoint_radius) { if (is_negative(_desired_speed)) { _desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f); } else { _desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f); } _yaw_error_cd = 0.0f; } else { // P controller with hard-coded gain to convert distance to desired speed // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise); // calculate bearing to destination _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); // if destination is behind vehicle, reverse towards it if (fabsf(_yaw_error_cd) > 9000) { _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _desired_speed = -_desired_speed; } // reduce desired speed if yaw_error is large // note: copied from calc_reduced_speed_for_turn_or_distance float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f); _desired_speed *= yaw_error_ratio; } // run steering and throttle controllers calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0); calc_throttle(_desired_speed, false, true); // mark us as in_reverse when using a negative throttle // To-Do: only in reverse if vehicle is actually travelling backwards? rover.set_reverse(_desired_speed < 0); }