// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Sub::guided_set_destination(const Location_Class& dest_loc)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
// set guided mode posvel target
bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
posvel_update_time_ms = AP_HAL::millis();
posvel_pos_target_cm = destination;
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
}
void Rover::nav_set_speed()
{
// if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0.0f;
prev_WP = current_loc;
next_WP = current_loc;
set_guided_WP(current_loc); // exit Guided_Velocity to prevent spam
return;
}
prev_WP = current_loc;
next_WP = current_loc;
const int32_t steer_value = steerController.get_steering_out_rate(guided_control.target_steer_speed);
location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f); // put the next wp at 4m forward at steer direction
nav_controller->update_waypoint(current_loc, next_WP);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
calc_throttle(guided_control.target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
}
// set guided mode angle target
void Copter::guided_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) {
guided_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)) {
set_auto_armed(true);
}
// log target
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));
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
// record velocity target
guided_vel_target_cms = velocity;
vel_update_time_ms = millis();
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
// set guided mode posvel target
void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) {
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
posvel_update_time_ms = millis();
posvel_pos_target_cm = destination;
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
vel_update_time_ms = millis();
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
// set guided mode angle target
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
guided_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.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// log target
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));
}
void Rover::nav_set_yaw_speed()
{
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0.0f;
return;
}
const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
// speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed
// 1 is double the cruise speed.
const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
calc_throttle(target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
}
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Copter::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
void Rover::update_current_mode(void)
{
switch (control_mode){
case AUTO:
case RTL:
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
case GUIDED: {
switch (guided_mode){
case Guided_Angle:
nav_set_yaw_speed();
break;
case Guided_WP:
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), Vector3f(g.speed_cruise, channel_throttle->get_servo_out(), 0));
}
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
}
case STEERING: {
/*
in steering mode we control lateral acceleration
directly. We first calculate the maximum lateral
acceleration at full steering lock for this speed. That is
V^2/R where R is the radius of turn. We get the radius of
turn from half the STEER2SRV_P.
*/
float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
calc_nav_steer();
// and throttle gives speed in proportion to cruise speed, up
// to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise;
set_reverse(target_speed < 0);
if (in_reverse) {
target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
} else {
target_speed = constrain_float(target_speed, 0, g.speed_cruise);
}
calc_throttle(target_speed);
break;
}
case LEARNING:
case MANUAL:
/*
in both MANUAL and LEARNING we pass through the
controls. Setting servo_out here actually doesn't matter, as
we set the exact value in set_servos(), but it helps for
logging
*/
channel_throttle->set_servo_out(channel_throttle->get_control_in());
channel_steer->set_servo_out(channel_steer->pwm_to_angle());
// mark us as in_reverse when using a negative throttle to
// stop AHRS getting off
set_reverse(channel_throttle->get_servo_out() < 0);
break;
case HOLD:
// hold position - stop motors and center steering
channel_throttle->set_servo_out(0);
channel_steer->set_servo_out(0);
break;
case INITIALISING:
break;
}
}
