/// desired_vel_to_pos - move position target using desired velocities
void AC_PosControl::desired_vel_to_pos(float nav_dt)
{
Vector2f target_vel_adj;
float vel_desired_total;
// range check nav_dt
if( nav_dt < 0 ) {
return;
}
// constrain and scale the desired velocity
vel_desired_total = safe_sqrt(_vel_desired.x*_vel_desired.x + _vel_desired.y*_vel_desired.y);
if (vel_desired_total > _speed_cms && vel_desired_total > 0.0f) {
_vel_desired.x = _speed_cms * _vel_desired.x/vel_desired_total;
_vel_desired.y = _speed_cms * _vel_desired.y/vel_desired_total;
}
// update target position
_pos_target.x += _vel_desired.x * nav_dt;
_pos_target.y += _vel_desired.y * nav_dt;
}
/// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_PosControl::rate_to_accel_xy(float dt)
{
const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s
float accel_total; // total acceleration in cm/s/s
// reset accel limit flag
_limit.accel_xy = false;
// reset last velocity if this controller has just been engaged or dt is zero
if (dt == 0.0) {
_accel_target.x = 0;
_accel_target.y = 0;
} else {
// feed forward desired acceleration calculation
_accel_target.x = (_vel_target.x - _vel_last.x)/dt;
_accel_target.y = (_vel_target.y - _vel_last.y)/dt;
}
// store this iteration's velocities for the next iteration
_vel_last.x = _vel_target.x;
_vel_last.y = _vel_target.y;
// calculate velocity error
_vel_error.x = _vel_target.x - vel_curr.x;
_vel_error.y = _vel_target.y - vel_curr.y;
// combine feed forward accel with PID output from velocity error
// To-Do: check accel limit flag before adding I term
_accel_target.x += _pid_rate_lat.get_pid(_vel_error.x, dt);
_accel_target.y += _pid_rate_lon.get_pid(_vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
// To-Do: move this check down to the accel_to_lean_angle method?
accel_total = safe_sqrt(_accel_target.x*_accel_target.x + _accel_target.y*_accel_target.y);
if (accel_total > POSCONTROL_ACCEL_XY_MAX) {
_accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;
_accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;
_limit.accel_xy = true; // unused
}
}
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
// should be called after _dcm_matrix is updated
void BC_AHRS::update_trig(void)
{
Vector2f yaw_vector;
const Matrix3f &temp = get_dcm_matrix();
// sin_yaw, cos_yaw
yaw_vector.x = temp.a.x;
yaw_vector.y = temp.b.x;
yaw_vector.normalize();
_sin_yaw = constrain_float(yaw_vector.y, -1.0, 1.0);
_cos_yaw = constrain_float(yaw_vector.x, -1.0, 1.0);
// cos_roll, cos_pitch
_cos_pitch = safe_sqrt(1 - (temp.c.x * temp.c.x));
_cos_roll = temp.c.z / _cos_pitch;
_cos_pitch = constrain_float(_cos_pitch, 0, 1.0);
_cos_roll = constrain_float(_cos_roll, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing,which it does do in avr-libc
// sin_roll, sin_pitch
_sin_pitch = -temp.c.x;
_sin_roll = temp.c.y / _cos_pitch;
}
/*
calculate a new aerodynamic_load_factor and limit nav_roll_cd to
ensure that the load factor does not take us below the sustainable
airspeed
*/
void Plane::update_load_factor(void)
{
float demanded_roll = fabsf(nav_roll_cd*0.01f);
if (demanded_roll > 85) {
// limit to 85 degrees to prevent numerical errors
demanded_roll = 85;
}
aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));
if (!aparm.stall_prevention) {
// stall prevention is disabled
return;
}
if (fly_inverted()) {
// no roll limits when inverted
return;
}
float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
if (max_load_factor <= 1) {
// our airspeed is below the minimum airspeed. Limit roll to
// 25 degrees
nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
roll_limit_cd = constrain_int32(roll_limit_cd, -2500, 2500);
} else if (max_load_factor < aerodynamic_load_factor) {
// the demanded nav_roll would take us past the aerodymamic
// load limit. Limit our roll to a bank angle that will keep
// the load within what the airframe can handle. We always
// allow at least 25 degrees of roll however, to ensure the
// aircraft can be maneuvered with a bad airspeed estimate. At
// 25 degrees the load factor is 1.1 (10%)
int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
if (roll_limit < 2500) {
roll_limit = 2500;
}
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
roll_limit_cd = constrain_int32(roll_limit_cd, -roll_limit, roll_limit);
}
}
// set desired location
bool AR_WPNav::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// set origin to last destination if waypoint controller active
if (is_active() && _orig_and_dest_valid && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
if (!get_stopping_location(_origin)) {
return false;
}
}
// initialise some variables
_destination = destination;
_orig_and_dest_valid = true;
_reached_destination = false;
update_distance_and_bearing_to_destination();
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, AR_WPNAV_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (fabsf(turn_angle_cd) < 10.0f) {
// if turning less than 0.1 degrees vehicle can continue at full speed
// we use 0.1 degrees instead of zero to avoid divide by zero in calcs below
_desired_speed_final = _desired_speed;
} else if (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(_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(_turn_max_mss * radius_m));
}
}
return true;
}
float
Compass::calculate_heading(const Matrix3f &dcm_matrix)
{
float headX;
float headY;
float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
float heading;
// sinf(pitch) = - dcm_matrix(3,1)
// cosf(pitch)*sinf(roll) = - dcm_matrix(3,2)
// cosf(pitch)*cosf(roll) = - dcm_matrix(3,3)
if (cos_pitch == 0.0f) {
// we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again.
return 0;
}
// Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
// Tilt compensated magnetic field Y component:
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain(atan2f(-headY,headX), -3.15f, 3.15f);
// Declination correction (if supplied)
if( fabsf(_declination) > 0.0f )
{
heading = heading + _declination;
if (heading > PI) // Angle normalization (-180 deg, 180 deg)
heading -= (2.0f * PI);
else if (heading < -PI)
heading += (2.0f * PI);
}
return heading;
}
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
{
// calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_z_cms;
if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
}
if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
}
accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
_accel_last_z_cms += jerk_z * dt;
_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
float vel_change_limit = _accel_last_z_cms * dt;
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
_flags.use_desvel_ff_z = true;
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_down?
if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += _vel_desired.z * dt;
}
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
// decelerate feed forward to zero
_vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
}
}
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
{
Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s
Vector3f vel_error; // The velocity error in cm/s.
float accel_total; // total acceleration in cm/s/s
// reset last velocity if this controller has just been engaged or dt is zero
if( dt == 0.0 ) {
desired_accel.x = 0;
desired_accel.y = 0;
} else {
// feed forward desired acceleration calculation
desired_accel.x = (vel_lat - _vel_last.x)/dt;
desired_accel.y = (vel_lon - _vel_last.y)/dt;
}
// store this iteration's velocities for the next iteration
_vel_last.x = vel_lat;
_vel_last.y = vel_lon;
// calculate velocity error
vel_error.x = vel_lat - vel_curr.x;
vel_error.y = vel_lon - vel_curr.y;
// combine feed foward accel with PID outpu from velocity error
desired_accel.x += _pid_rate_lat->get_pid(vel_error.x, dt);
desired_accel.y += _pid_rate_lon->get_pid(vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
accel_total = safe_sqrt(desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y);
if( accel_total > WPNAV_ACCEL_MAX ) {
desired_accel.x = WPNAV_ACCEL_MAX * desired_accel.x/accel_total;
desired_accel.y = WPNAV_ACCEL_MAX * desired_accel.y/accel_total;
}
// call accel based controller with desired acceleration
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
}
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
// this should be called whenever the radius or rate are changed
// initialises the yaw and current position around the circle
void AC_Circle::calc_velocities(bool init_velocity)
{
// if we are doing a panorama set the circle_angle to the current heading
if (_radius <= 0) {
_angular_vel_max = ToRad(_rate);
_angular_accel = max(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second
}else{
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));
// angular_velocity in radians per second
_angular_vel_max = velocity_max/_radius;
_angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);
// angular_velocity in radians per second
_angular_accel = max(_pos_control.get_accel_xy()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN));
}
// initialise angular velocity
if (init_velocity) {
_angular_vel = 0;
}
}
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_lateral_acceleration and before calc_throttle
// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
{
// this method makes use the following internal variables
const float yaw_error_cd = _yaw_error_cd;
const float target_lateral_accel_G = _desired_lat_accel;
const float distance_to_waypoint = _distance_to_destination;
// calculate the yaw_error_ratio which is the error (capped at 90degrees) expressed as a ratio (from 0 ~ 1)
float yaw_error_ratio = constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f);
// apply speed_turn_gain parameter (expressed as a percentage) to yaw_error_ratio
yaw_error_ratio *= (100 - g.speed_turn_gain) * 0.01f;
// calculate absolute lateral acceleration expressed as a ratio (from 0 ~ 1) of the vehicle's maximum lateral acceleration
const float lateral_accel_ratio = constrain_float(fabsf(target_lateral_accel_G / (g.turn_max_g * GRAVITY_MSS)), 0.0f, 1.0f);
// calculate a lateral acceleration based speed scaling
const float lateral_accel_speed_scaling = 1.0f - lateral_accel_ratio * yaw_error_ratio;
// calculate a pivot steering based speed scaling (default to no reduction)
float pivot_speed_scaling = 1.0f;
if (rover.use_pivot_steering(yaw_error_cd)) {
pivot_speed_scaling = 0.0f;
}
// scaled speed
float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling);
// limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_accel_max())) {
const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_accel_max() + sq(_desired_speed_final));
speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max);
}
// return minimum speed
return speed_scaled;
}
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point
/// results placed in stopping_position vector
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{
Vector3f curr_pos = _inav.get_position();
Vector3f curr_vel = _inav.get_velocity();
float linear_distance; // the distance at which we swap from a linear to sqrt response
float linear_velocity; // the velocity above which we swap from a linear to sqrt response
float stopping_dist; // the distance within the vehicle can stop
float kP = _p_pos_xy.kP();
// calculate current velocity
float vel_total = safe_sqrt(curr_vel.x*curr_vel.x + curr_vel.y*curr_vel.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (vel_total < 10.0f || kP <= 0.0f || _accel_cms <= 0.0f) {
stopping_point = curr_pos;
return;
}
// calculate point at which velocity switches from linear to sqrt
linear_velocity = _accel_cms/kP;
// calculate distance within which we can stop
if (vel_total < linear_velocity) {
stopping_dist = vel_total/kP;
} else {
linear_distance = _accel_cms/(2.0f*kP*kP);
stopping_dist = linear_distance + (vel_total*vel_total)/(2.0f*_accel_cms);
}
// constrain stopping distance
stopping_dist = constrain_float(stopping_dist, 0, _leash);
// convert the stopping distance into a stopping point using velocity vector
stopping_point.x = curr_pos.x + (stopping_dist * curr_vel.x / vel_total);
stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total);
}
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_COMMAND_INT: {
// decode packet
mavlink_command_int_t packet;
mavlink_msg_command_int_decode(msg, &packet);
MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
switch (packet.command) {
case MAV_CMD_DO_SET_HOME: {
// assume failure
result = MAV_RESULT_FAILED;
if (is_equal(packet.param1, 1.0f)) {
// if param1 is 1, use current location
if (rover.set_home_to_current_location(true)) {
result = MAV_RESULT_ACCEPTED;
}
break;
}
// ensure param1 is zero
if (!is_zero(packet.param1)) {
break;
}
// check frame type is supported
if (packet.frame != MAV_FRAME_GLOBAL &&
packet.frame != MAV_FRAME_GLOBAL_INT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
break;
}
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
break;
}
Location new_home_loc {};
new_home_loc.lat = packet.x;
new_home_loc.lng = packet.y;
new_home_loc.alt = packet.z * 100;
// handle relative altitude
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (!rover.ahrs.home_is_set()) {
// cannot use relative altitude if home is not set
break;
}
new_home_loc.alt += rover.ahrs.get_home().alt;
}
if (rover.set_home(new_home_loc, true)) {
result = MAV_RESULT_ACCEPTED;
}
break;
}
#if MOUNT == ENABLED
case MAV_CMD_DO_SET_ROI: {
// param1 : /* Region of interest mode (not used)*/
// param2 : /* MISSION index/ target ID (not used)*/
// param3 : /* ROI index (not used)*/
// param4 : /* empty */
// x : lat
// y : lon
// z : alt
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
break;
}
Location roi_loc;
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
}
#endif
default:
result = MAV_RESULT_UNSUPPORTED;
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
MAV_RESULT result = MAV_RESULT_UNSUPPORTED;
// do command
switch (packet.command) {
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
#if MOUNT == ENABLED
// Sets the region of interest (ROI) for the camera
case MAV_CMD_DO_SET_ROI:
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
#endif
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
#endif
break;
case MAV_CMD_MISSION_START:
rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1, 3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1, 1.0f)) {
// run pre_arm_checks and arm_checks and display failures
if (rover.arm_motors(AP_Arming::MAVLINK)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_zero(packet.param1)) {
if (rover.disarm_motors()) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_FENCE_ENABLE:
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1) {
case 0:
rover.g2.fence.enable(false);
break;
case 1:
rover.g2.fence.enable(true);
break;
default:
result = MAV_RESULT_FAILED;
break;
}
break;
case MAV_CMD_DO_SET_HOME:
{
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1, 1.0f)) {
if (rover.set_home_to_current_location(true)) {
result = MAV_RESULT_ACCEPTED;
}
} else {
// ensure param1 is zero
if (!is_zero(packet.param1)) {
break;
}
Location new_home_loc {};
new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f);
new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f);
new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f);
if (rover.set_home(new_home_loc, true)) {
result = MAV_RESULT_ACCEPTED;
}
}
break;
}
case MAV_CMD_NAV_SET_YAW_SPEED:
{
// param1 : yaw angle to adjust direction by in centidegress
// param2 : Speed - normalized to 0 .. 1
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// send yaw change and target speed to guided mode controller
const float speed_max = rover.control_mode->get_speed_default();
const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max);
rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed);
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED;
if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_MOTOR_TEST:
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
// param3 : throttle (range depends upon param2)
// param4 : timeout (in seconds)
result = rover.mavlink_motor_test_start(chan, static_cast<uint8_t>(packet.param1),
static_cast<uint8_t>(packet.param2),
static_cast<int16_t>(packet.param3),
packet.param4);
break;
default:
result = handle_command_long_message(packet);
break;
}
mavlink_msg_command_ack_send_buf(
msg,
chan,
packet.command,
result);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
break;
}
mavlink_rc_channels_override_t packet;
mavlink_msg_rc_channels_override_decode(msg, &packet);
RC_Channels::set_override(0, packet.chan1_raw);
RC_Channels::set_override(1, packet.chan2_raw);
RC_Channels::set_override(2, packet.chan3_raw);
RC_Channels::set_override(3, packet.chan4_raw);
RC_Channels::set_override(4, packet.chan5_raw);
RC_Channels::set_override(5, packet.chan6_raw);
RC_Channels::set_override(6, packet.chan7_raw);
RC_Channels::set_override(7, packet.chan8_raw);
rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
break;
}
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
if (packet.target != rover.g.sysid_this_mav) {
break; // only accept control aimed at us
}
const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f;
const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f;
RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll);
RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle);
rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if (msg->sysid != rover.g.sysid_my_gcs) {
break;
}
rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
break;
}
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// ensure type_mask specifies to use thrust
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
break;
}
// convert thrust to ground speed
packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
// if the body_yaw_rate field is ignored, convert quaternion to heading
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
// convert quaternion to heading
float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
} else {
// use body_yaw_rate field
rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
switch (packet.coordinate_frame) {
case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_NED: {
// rotate from body-frame to NE frame
const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
// add offset to current location
location_offset(target_loc, ne_x, ne_y);
}
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location
location_offset(target_loc, packet.x, packet.y);
break;
default:
// MAV_FRAME_LOCAL_NED interpret as an offset from home
target_loc = rover.ahrs.get_home();
location_offset(target_loc, packet.x, packet.y);
break;
}
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw heading
if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
// sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) {
// result = MAV_RESULT_FAILED;
break;
}
target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw heading
if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame is provided in MAV_FRAME_GLOBAL_xxx
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
}
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE
#if MOUNT == ENABLED
// deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
rover.camera_mount.configure_msg(msg);
break;
}
// deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
rover.camera_mount.control_msg(msg);
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
break;
}
// send or receive fence points with GCS
case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160
case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
rover.g2.fence.handle_msg(*this, msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
rover.rangefinder.handle_msg(msg);
rover.g2.proximity.handle_msg(msg);
break;
default:
handle_common_message(msg);
break;
} // end switch
} // end handle mavlink
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
rate_change_limit = _accel_rp_max * _dt;
float rate_ef_desired;
float angle_to_target;
if (_accel_rp_max > 0.0f) {
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
// update earth-frame roll angle target using desired roll rate
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error);
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = pitch_angle_ef - _angle_ef_target.y;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
// update earth-frame pitch angle target using desired pitch rate
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
float rate_change = yaw_rate_ef - _rate_ef_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.z += rate_change;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = 0;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
/// pos_to_rate_xy - horizontal position error to velocity controller
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
{
Vector3f curr_pos = _inav.get_position();
float linear_distance; // the distance we swap between linear and sqrt velocity response
float kP = _p_pos_xy.kP();
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
// constrain target position to within reasonable distance of current location
_distance_to_target = pythagorous2(_pos_error.x, _pos_error.y);
if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
_pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;
_pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
// re-calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
_distance_to_target = _leash;
}
// calculate the distance at which we swap between linear and sqrt velocity response
linear_distance = _accel_cms/(2.0f*kP*kP);
if (_distance_to_target > 2.0f*linear_distance) {
// velocity response grows with the square root of the distance
float vel_sqrt = safe_sqrt(2.0f*_accel_cms*(_distance_to_target-linear_distance));
_vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target;
_vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target;
}else{
// velocity response grows linearly with the distance
_vel_target.x = _p_pos_xy.kP() * _pos_error.x;
_vel_target.y = _p_pos_xy.kP() * _pos_error.y;
}
// decide velocity limit due to position error
float vel_max_from_pos_error;
if (use_desired_rate) {
// if desired velocity (i.e. velocity feed forward) is being used we limit the maximum velocity correction due to position error to 2m/s
vel_max_from_pos_error = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR;
}else{
// if desired velocity is not used, we allow position error to increase speed up to maximum speed
vel_max_from_pos_error = _speed_cms;
}
// scale velocity to stays within limits
float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
if (vel_total > vel_max_from_pos_error) {
_vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;
_vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;
}
// add desired velocity (i.e. feed forward).
if (use_desired_rate) {
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
}
}
}
void Copter::ModeFollow::run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
// position and velocity requests will be ignored while the vehicle is not in guided mode
// variables to be sent to velocity controller
Vector3f desired_velocity_neu_cms;
bool use_yaw = false;
float yaw_cd = 0.0f;
Vector3f dist_vec; // vector to lead vehicle
Vector3f dist_vec_offs; // vector to lead vehicle + offset
Vector3f vel_of_target; // velocity of lead vehicle
if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
// convert dist_vec_offs to cm in NEU
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
// calculate desired velocity vector in cm/s in NEU
const float kp = g2.follow.get_pos_p().kP();
desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) {
const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_max_speed_xy();
}
// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up());
// unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu;
const float dir_to_target_neu_len = dir_to_target_neu.length();
if (!is_zero(dir_to_target_neu_len)) {
dir_to_target_neu /= dir_to_target_neu_len;
}
// create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
// create horizontal unit vector towards target (required for slow down calculations)
Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
if (!dir_to_target_xy.is_zero()) {
dir_to_target_xy.normalize();
}
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// limit the horizontal velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt);
// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate
desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z);
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
if (dist_vec_xy.length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
float target_hdg = 0.0f;
if (g2.follow.get_target_heading_deg(target_hdg)) {
yaw_cd = target_hdg * 100.0f;
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_NONE:
default:
// do nothing
break;
}
}
// log output at 10hz
uint32_t now = AP_HAL::millis();
bool log_request = false;
if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
log_request = true;
last_log_ms = now;
}
// re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run();
}
/// pos_to_rate_xy - horizontal position error to velocity controller
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainScaler)
{
Vector3f curr_pos = _inav.get_position();
float linear_distance; // the distance we swap between linear and sqrt velocity response
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
// constrain target position to within reasonable distance of current location
_distance_to_target = norm(_pos_error.x, _pos_error.y);
if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
_pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;
_pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
// re-calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
_distance_to_target = _leash;
}
// calculate the distance at which we swap between linear and sqrt velocity response
linear_distance = _accel_cms/(2.0f*kP*kP);
if (_distance_to_target > 2.0f*linear_distance) {
// velocity response grows with the square root of the distance
float vel_sqrt = safe_sqrt(2.0f*_accel_cms*(_distance_to_target-linear_distance));
_vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target;
_vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target;
}else{
// velocity response grows linearly with the distance
_vel_target.x = kP * _pos_error.x;
_vel_target.y = kP * _pos_error.y;
}
if (mode == XY_MODE_POS_LIMITED_AND_VEL_FF) {
// this mode is for loiter - rate-limiting the position correction
// allows the pilot to always override the position correction in
// the event of a disturbance
// scale velocity within limit
float vel_total = norm(_vel_target.x, _vel_target.y);
if (vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR) {
_vel_target.x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.x/vel_total;
_vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/vel_total;
}
// add velocity feed-forward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
} else {
if (mode == XY_MODE_POS_AND_VEL_FF) {
// add velocity feed-forward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
}
// scale velocity within speed limit
float vel_total = norm(_vel_target.x, _vel_target.y);
if (vel_total > _speed_cms) {
_vel_target.x = _speed_cms * _vel_target.x/vel_total;
_vel_target.y = _speed_cms * _vel_target.y/vel_total;
}
}
}
}
/// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
float track_covered; // distance (in cm) along the track that the vehicle has traveled. Measured by drawing a perpendicular line from the track to the vehicle.
Vector3f track_error; // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle
float track_desired_max; // the farthest distance (in cm) along the track that the leash will allow
float track_leash_slack; // additional distance (in cm) along the track from our track_covered position that our leash will allow
bool reached_leash_limit = false; // true when track has reached leash limit and we need to slow down the target point
// get current location
Vector3f curr_pos = _inav.get_position();
// calculate terrain adjustments
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate 3d vector from segment's origin
Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin;
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
// calculate the point closest to the vehicle on the segment from origin to destination
Vector3f track_covered_pos = _pos_delta_unit * track_covered;
// calculate the distance vector from the vehicle to the closest point on the segment from origin to destination
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get up leash if we are moving up, down leash if we are moving down
float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z() : _pos_control.get_leash_down_z();
// use pythagoras's theorem calculate how far along the track we could move the intermediate target before reaching the end of the leash
// track_desired_max is the distance from the vehicle to our target point along the track. It is the "hypotenuse" which we want to be no longer than our leash (aka _track_leash_length)
// track_error is the line from the vehicle to the closest point on the track. It is the "opposite" side
// track_leash_slack is the line from the closest point on the track to the target point. It is the "adjacent" side. We adjust this so the track_desired_max is no longer than the leash
float track_leash_length_abs = fabsf(_track_leash_length);
float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
track_desired_max = track_covered + track_leash_slack;
// check if target is already beyond the leash
if (_track_desired > track_desired_max) {
reached_leash_limit = true;
}
// get current velocity
const Vector3f &curr_vel = _inav.get_velocity();
// get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms;
float kP = _pos_control.get_pos_xy_p().kP();
if (is_positive(kP)) { // avoid divide by zero
linear_velocity = _track_accel/kP;
}
// let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity) {
// we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0;
}else{
// increase intermediate target point's velocity if not yet at the leash limit
if(dt > 0 && !reached_leash_limit) {
_limited_speed_xy_cms += 2.0f * _track_accel * dt;
}
// do not allow speed to be below zero or over top speed
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);
// check if we should begin slowing down
if (!_flags.fast_waypoint) {
float dist_to_dest = _track_length - _track_desired;
if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) {
_flags.slowing_down = true;
}
// if target is slowing down, limit the speed
if (_flags.slowing_down) {
_limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel));
}
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
}
}
// advance the current target
if (!reached_leash_limit) {
_track_desired += _limited_speed_xy_cms * dt;
// reduce speed if we reach end of leash
if (_track_desired > track_desired_max) {
_track_desired = track_desired_max;
_limited_speed_xy_cms -= 2.0f * _track_accel * dt;
if (_limited_speed_xy_cms < 0.0f) {
_limited_speed_xy_cms = 0.0f;
}
}
}
// do not let desired point go past the end of the track unless it's a fast waypoint
if (!_flags.fast_waypoint) {
_track_desired = constrain_float(_track_desired, 0, _track_length);
} else {
_track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);
}
// recalculate the desired position
Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
// convert final_target.z to altitude above the ekf origin
final_target.z += terr_offset;
_pos_control.set_pos_target(final_target);
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
if( _track_desired >= _track_length ) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint) {
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}
// update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
// if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
set_yaw_cd(get_bearing_cd(_origin, _destination));
} else {
Vector3f horiz_leash_xy = final_target - curr_pos;
horiz_leash_xy.z = 0;
if (horiz_leash_xy.length() > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x)));
}
}
}
// successfully advanced along track
return true;
}
void pf_field::update(local_member* p_ac, AP_AHRS_DCM* ahrs, AP_Airspeed* airspeed)
{
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
void pf_field::update(local_member* p_ac, AP_AHRS_HIL* ahrs, AP_Airspeed* airspeed)
{
#endif
const Relative* tmp_p_rel = p_ac->get_rel(); //Gets the current relative information from the ac object
Vector3i tmp_gV_L = *p_ac->get_gV_L();
uint16_t tmp_psi_ac = *p_ac->get_hdg(); //Get our heading (deg)
//Calculate total repulsive function from i flock members
Vector3f tmp_r_phi; //initialize vector (NED components)
//Sum repulsive functions from i flock members
for(int i=0;i<tmp_p_rel->Num_members;i++)
{
//indexing convention to organize member pointers
int j = tmp_p_rel->Member_ids[i]-1;
//Relative distance to flock members (NED components)
Vector3f tmp_dX; //initialize vector each iteration
tmp_dX.x=tmp_p_rel->dX[j]/100.0; //[m]
tmp_dX.y=tmp_p_rel->dY[j]/100.0; //[m]
tmp_dX.z=tmp_p_rel->dZ[j]/100.0; //[m]
//Calculate repulsive parameters based on relative states
//Gaussian repulsive function accumulation
tmp_r_phi.x += (-2*(tmp_dX.x)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.y += (-2*(tmp_dX.y)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.z += (-2*(tmp_dX.z)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
}
_phi_r = tmp_r_phi; //Repulsive potential gradient vector (NED component)
//Relative distance vector to leader (NED components)
Vector3i tmp_dL; //initialize vector (NED components)
tmp_dL.x=tmp_p_rel->dXL; //[m*100]
tmp_dL.y=tmp_p_rel->dYL; //[m*100]
tmp_dL.z=tmp_p_rel->dZL; //[m*100]
//Relative distance magnitude to leader
int32_t tmp_d2L=tmp_p_rel->d2L; //[m*100]
//Relative velocity to leader (NED components)
//Vector3i tmp_dV; //initialize vector (NED components)
Vector3i tmp_gV_LNAV; //initialize dVaspd (Local Navigation frame)
//tmp_dV.x=tmp_p_rel->dVXL; //[m/s*100]
//tmp_dV.y=tmp_p_rel->dVYL; //[m/s*100]
//tmp_dV.z=tmp_p_rel->dVZL; //[m/s*100]
float Cpsi = cos(ToRad(tmp_psi_ac/100.0));
float Spsi = sin(ToRad(tmp_psi_ac/100.0));
tmp_gV_LNAV.x=int16_t((tmp_gV_L.x)*Cpsi+(tmp_gV_L.y)*Spsi); //[m/s*100]
tmp_gV_LNAV.y=int16_t(-(tmp_gV_L.x)*Spsi+(tmp_gV_L.y)*Cpsi); //[m/s*100]
tmp_gV_LNAV.z=tmp_p_rel->dVZL; //[m/s*100]
//Calculate the attractive potential gradient (NED components)
/*Note:
Attractive potential is comprised of two types of potential functions
-In the far-field regime we use a linear potential field
-In the near-field regime we use a quadratic potential field
These fields must be matched at _chi, the near-field, far-field regime cut-off such that
-Gradient of quadratic at _chi = gradient of linear at _chi
In far-field, we are guided towards the leader's position (no offset) - this should improve convergence
In near-field, we are guided towards the offset position, the side of which is dictated by swarm algorithm
*/
Vector3i tmp_pf_offset_NED; //Initialize vector
uint16_t tmp_psi_L = tmp_p_rel->hdgL; //Get the heading of the Leader
float CpsiL = cos(ToRad(tmp_p_rel->hdgL/100.0));
float SpsiL = sin(ToRad(tmp_p_rel->hdgL/100.0));
//set offset (Leader's Local frame)
Vector3i tmp_pf_offset = _pf_offset_l;
//update offset with side correction
tmp_pf_offset.y = tmp_pf_offset.y*_side;
//Rotate the offset distances (Leader's Local Navigation Frame) to NED frame
tmp_pf_offset_NED.x = tmp_pf_offset.x*CpsiL - tmp_pf_offset.y*SpsiL;
tmp_pf_offset_NED.y = tmp_pf_offset.x*SpsiL + tmp_pf_offset.y*CpsiL;
tmp_pf_offset_NED.z = tmp_pf_offset.z;
//Extrapolate Goal for NF case
Location tmp_goal_loc = *p_ac->get_loc();
Location tmp_current_loc = *p_ac->get_loc();
//A bunch of new stuff for this extrapolation technique
location_offset(&tmp_goal_loc,tmp_dL.x/100.0, tmp_dL.y/100.0);
location_offset(&tmp_goal_loc,-tmp_pf_offset_NED.x/100.0, -tmp_pf_offset_NED.y/100.0);
float tmp_N_extrap = 10*CpsiL;
float tmp_E_extrap = 10*SpsiL;
location_offset(&tmp_goal_loc,tmp_N_extrap, tmp_E_extrap);
int32_t tmp_B_extrap = get_bearing_cd(&tmp_current_loc,&tmp_goal_loc);
Vector3f extrap_uvec_NED;
extrap_uvec_NED.x = cos(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.y = sin(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.z = 0;
tmp_dL -=tmp_pf_offset_NED;
tmp_d2L = int32_t(100*safe_sqrt(square(tmp_dL.x/100.0)+square(tmp_dL.y/100.0)+square(tmp_dL.z/100.0)));
//Subtract offset distance vector from leader distance vector to get modified relative distance vector to leader
if(tmp_d2L<(_chi*100)) //Near-field regime condition
{
_phi_a.x = 2*(_lambda.x/100.0)*(tmp_dL.x/100.0); //Calculate Quad PF X gradient with weighting parameter (NED frame)
_phi_a.y = 2*(_lambda.y/100.0)*(tmp_dL.y/100.0); //Calculate Quad PF Y gradient with weighting parameter (NED frame)
_phi_a.z = 2*(_lambda.z/100.0)*(tmp_dL.z/100.0); //Calculate Quad PF Z gradient with weighting parameter (NED frame)
float tmp_phi_a_mag = _phi_a.length();
_phi_a_c.x = tmp_phi_a_mag*extrap_uvec_NED.x;
_phi_a_c.y = tmp_phi_a_mag*extrap_uvec_NED.y;
_phi_a_c.z = 0;
}
else
{
//float tmp_M =2*safe_sqrt(square(_lambda.x/100.0)+square(_lambda.y/100.0)+square(_lambda.z/100.0))*(_chi); //Slope of linear potential field (Magnitude of gradient) = nominal magnitude of quadratic gradient at chi
float tmp_M = 2*_chi;
float tmp_R = safe_sqrt(square((_lambda.x/100.0)*(tmp_dL.x/100.0))+square((_lambda.y/100.0)*(tmp_dL.y/100.0))+square((_lambda.z/100.0)*(tmp_dL.z/100)));
_phi_a.x = tmp_M*square(_lambda.x/100.0)*(tmp_dL.x/100.0)/tmp_R; //Calculate Lin PF X gradient with weighting parameter (NED frame)
_phi_a.y = tmp_M*square(_lambda.y/100.0)*(tmp_dL.y/100.0)/tmp_R; //Calculate Lin PF Y gradient with weighting parameter (NED frame)
_phi_a.z = tmp_M*square(_lambda.z/100.0)*(tmp_dL.z/100.0)/tmp_R; //Calculate Lin PF Z gradient with weighting parameter (NED frame)
}
//Calculate the total potential gradient components
_phi_NED = _phi_a + _phi_r;
_phi_c_NED = _phi_a_c + _phi_r;
//Calculate the magnitude of the total potential function gradient
if(_phi_NED.length() > 0) //divide-by-zero check (safety first)
{
//Calculate the normalized potential gradient components
_Nphi_NED = _phi_NED.normalized();
_Nphi_c_NED = _phi_c_NED.normalized();
//Find X component in Local Navigation frame by rotating NED vector about heading
_Nphi_l.x = _Nphi_NED.x*Cpsi + _Nphi_NED.y*Spsi;
if(tmp_d2L<_chi*100) //near-field consideration- We don't want the VWP behind the body
{
_regime_mask = 0x01; //Make first bit in regime mask 1 to reflect that we're in near-field regime
/* //Correct normalized vector for VWP in near-field
if(_Nphi_l.x<0) //Vector is pointing behind ac in Local Navigation frame
{
//Rotate NED pf gradiend components into Local Navigation components
_phi_l.x = _phi_NED.x*cos(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_l.y = - _phi_NED.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*cos(ToRad(tmp_psi_ac/100.0));
//Correct potential function to point forward
/* Note:
Corrected by using airspeed instead of relative distance... almost a predictor of where the goal "will" be in a second
/
float airspeed_est;
if(ahrs->airspeed_estimate(&airspeed_est))
{
_phi_l.x = 2*(_lambda.x/100.0)*(airspeed_est);
}
else
{
_phi_l.x = 2*(_lambda.x/100.0)*10; //Not the most elegant way to do this... 10 is an average airspeed for the skysurfer
}
//Corrected potential rotated back into NED frame from Local Navigation frame
_phi_c_NED.x= _phi_l.x*cos(ToRad(tmp_psi_ac/100.0)) - _phi_l.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_c_NED.y= _phi_l.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_l.y*cos(ToRad(tmp_psi_ac/100.0));
//Normalize the corrected potential gradient vector
_Nphi_c_NED = _phi_c_NED.normalized();
//Modify regime mask to reflect that both the first bit and the second bit are 1 (near-field regime, and using a corrected
_regime_mask = 0x03;
}
//Make sure regime mask reflects that only
else _regime_mask = 0x01; */
}
else
{
//Regime mask should reflect that we are not in the near-field, and we therefore aren't using a corrected potential field
_regime_mask = 0x00;
}
}
else
{
//If the magnitude of the gradient is not greater than 0, make the norm 0 to avoid divide by 0 problems
_Nphi_NED.zero();
_Nphi_c_NED.zero();
}
float tmp_dNorth_com;
float tmp_dEast_com;
float tmp_dalt_com;
float tmp_dspd_com;
if(tmp_d2L<_chi*100)
{
//Calculate the North and East Offset [m]
tmp_dNorth_com = _VWP_offset*_Nphi_c_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_c_NED.y;
//Calculate the change in altitude [m]
//k_alt_V is the equivalent of a derivative gain- set to 0 for simplicity.
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_c_NED;
}
else
{ //Far-Field Case
tmp_dNorth_com = _VWP_offset*_Nphi_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_NED.y;
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_NED;
}
const Location* p_current_location = p_ac->get_loc();
_next_VWP = *p_current_location;
location_offset(&_next_VWP, tmp_dNorth_com, tmp_dEast_com);
_next_VWP.alt+=(int32_t)(tmp_dalt_com*100); //double-check sign here.
constrain(_next_VWP.alt,PFG_MIN_ALT,PFG_MAX_ALT);
if(tmp_d2L<_chi*100)
{ //Near-Field Case
//New speed matching/ Speed from PFG command algorithm
_next_airspeed_com = int32_t(100*((tmp_gV_LNAV.x/100.0*(PFG_K_P_dV/100.0)) +(_Nphi_l.x*(PFG_K_I_dV/100.0))));
constrain(_next_airspeed_com,PFG_MIN_AIRSPEED_CM,PFG_MAX_AIRSPEED_CM);
}
else
{ //Far-Field Case
_next_airspeed_com = PFG_MAX_AIRSPEED_CM; //In far-field, set airspeed to maximum allowable (to catch up)
}
}
const Location* pf_field::get_VWP(){
return (const Location*)&_next_VWP;
}
const int32_t* pf_field::get_new_speed(){
return (const int32_t*)&_next_airspeed_com;
}
const Vector3f* pf_field::get_pfg_att(){
return (const Vector3f*)&_phi_a;
}
const Vector3f* pf_field::get_pfg_rep(){
return (const Vector3f*)&_phi_r;
}
const Vector3f* pf_field::get_pfg_norm(){
return (const Vector3f*)&_phi_norm;
}
uint8_t pf_field::get_regime_mask(){
return _regime_mask;
}
pf_field PF_FIELD;
/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin)
/// seg_type should be calculated by calling function based on the mission
bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
{
// mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000));
float dt = _pos_control.time_since_last_xy_update();
if (dt >= 0.2f) {
dt = 0.0f;
}
// check _wp_accel_cmss is reasonable to avoid divide by zero
if (_wp_accel_cmss <= 0) {
_wp_accel_cmss.set_and_save(WPNAV_ACCELERATION);
}
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
// _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// calculate spline velocity at origin
if (stopped_at_start || !prev_segment_exists) {
// if vehicle is stopped at the origin, set origin velocity to 0.02 * distance vector from origin to destination
_spline_origin_vel = (destination - origin) * dt;
_spline_time = 0.0f;
_spline_vel_scaler = 0.0f;
}else{
// look at previous segment to determine velocity at origin
if (_flags.segment_type == SEGMENT_STRAIGHT) {
// previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
// before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
_spline_origin_vel = (_destination - _origin);
_spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment?
_spline_vel_scaler = _pos_control.get_vel_target().length(); // start velocity target from current target velocity
}else{
// previous segment is splined, vehicle will fly through origin
// we can use the previous segment's destination velocity as this segment's origin velocity
// Note: previous segment will leave destination velocity parallel to position difference vector
// from previous segment's origin to this segment's destination)
_spline_origin_vel = _spline_destination_vel;
if (_spline_time > 1.0f && _spline_time < 1.1f) { // To-Do: remove hard coded 1.1f
_spline_time -= 1.0f;
}else{
_spline_time = 0.0f;
}
// Note: we leave _spline_vel_scaler as it was from end of previous segment
}
}
// calculate spline velocity at destination
switch (seg_end_type) {
case SEGMENT_END_STOP:
// if vehicle stops at the destination set destination velocity to 0.02 * distance vector from origin to destination
_spline_destination_vel = (destination - origin) * dt;
_flags.fast_waypoint = false;
break;
case SEGMENT_END_STRAIGHT:
// if next segment is straight, vehicle's final velocity should face along the next segment's position
_spline_destination_vel = (next_destination - destination);
_flags.fast_waypoint = true;
break;
case SEGMENT_END_SPLINE:
// if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
_spline_destination_vel = (next_destination - origin);
_flags.fast_waypoint = true;
break;
}
// code below ensures we don't get too much overshoot when the next segment is short
float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length();
float pos_len = (destination - origin).length() * 4.0f;
if (vel_len > pos_len) {
// if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocityscale the start and stop velocities down
float vel_scaling = pos_len / vel_len;
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
}else{
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
}
// store origin and destination locations
_origin = origin;
_destination = destination;
_terrain_alt = terrain_alt;
// calculate slow down distance
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cmss);
// get alt-above-terrain
float terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(terr_offset)) {
return false;
}
}
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin + Vector3f(0,0,terr_offset));
_flags.reached_destination = false;
_flags.segment_type = SEGMENT_SPLINE;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
_flags.wp_yaw_set = false;
// initialise yaw related variables
_track_length_xy = safe_sqrt(sq(_destination.x - _origin.x)+sq(_destination.y - _origin.y)); // horizontal track length (used to decide if we should update yaw)
return true;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
// if target velocity is zero the origin and destination must be the same
// so flag reached destination (and protect against divide by zero)
float target_vel_length = target_vel.length();
if (is_zero(target_vel_length)) {
_flags.reached_destination = true;
return true;
}
_pos_delta_unit = target_vel / target_vel_length;
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-_track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cmss);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cmss * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
_spline_time_scale = _spline_vel_scaler / target_vel_length;
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
// if the leash is very short (i.e. flying at low speed) use the target point's velocity along the track
if (!is_zero(target_vel.x) && !is_zero(target_vel.y)) {
set_yaw_cd(RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)));
}
} else {
// point vehicle along the leash (i.e. point vehicle towards target point on the segment from origin to destination)
float track_error_xy_length = safe_sqrt(sq(track_error.x)+sq(track_error.y));
if (track_error_xy_length > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
// To-Do: why is track_error sign reversed?
set_yaw_cd(RadiansToCentiDegrees(atan2f(-track_error.y,-track_error.x)));
}
}
}
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
bool
AP_GPS_SBF::process_message(void)
{
uint16_t blockid = (sbf_msg.blockid & 8191u);
Debug("BlockID %d", blockid);
switch (blockid) {
case ExtEventPVTGeodetic:
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
break;
case PVTGeodetic:
{
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
if (temp.WNc != 65535) {
state.time_week = temp.WNc;
state.time_week_ms = (uint32_t)(temp.TOW);
}
state.last_gps_time_ms = AP_HAL::millis();
// Update velocity state (don't use −2·10^10)
if (temp.Vn > -200000) {
state.velocity.x = (float)(temp.Vn);
state.velocity.y = (float)(temp.Ve);
state.velocity.z = (float)(-temp.Vu);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.rtk_age_ms = temp.MeanCorrAge * 10;
// value is expressed as twice the rms error = int16 * 0.01/2
state.horizontal_accuracy = (float)temp.HAccuracy * 0.005f;
state.vertical_accuracy = (float)temp.VAccuracy * 0.005f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
// Update position state (don't use −2·10^10)
if (temp.Latitude > -200000) {
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f);
}
if (temp.NrSV != 255) {
state.num_sats = temp.NrSV;
}
Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode);
switch (temp.Mode & 15) {
case 0: // no pvt
state.status = AP_GPS::NO_FIX;
break;
case 1: // standalone
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // dgps
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // fixed location
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // sbas
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 7: // moving rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 8: // moving rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
}
if ((temp.Mode & 64) > 0) { // gps is in base mode
state.status = AP_GPS::NO_FIX;
} else if ((temp.Mode & 128) > 0) { // gps only has 2d fix
state.status = AP_GPS::GPS_OK_FIX_2D;
}
return true;
}
case DOP:
{
const msg4001 &temp = sbf_msg.data.msg4001u;
state.hdop = temp.HDOP;
state.vdop = temp.VDOP;
break;
}
case ReceiverStatus:
{
const msg4014 &temp = sbf_msg.data.msg4014u;
RxState = temp.RxState;
RxError = temp.RxError;
break;
}
case VelCovGeodetic:
{
const msg5908 &temp = sbf_msg.data.msg5908u;
// select the maximum variance, as the EKF will apply it to all the columnds in it's estimate
// FIXME: Support returning the covariance matric to the EKF
float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));
if (is_positive(max_variance_squared)) {
state.have_speed_accuracy = true;
state.speed_accuracy = sqrt(max_variance_squared);
} else {
state.have_speed_accuracy = false;
}
break;
}
}
return false;
}
bool
AP_GPS_SBF::process_message(void)
{
uint16_t blockid = (sbf_msg.blockid & 4095u);
Debug("BlockID %d", blockid);
// ExtEventPVTGeodetic
if (blockid == 4038) {
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
}
// PVTGeodetic
if (blockid == 4007) {
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
if (temp.WNc != 65535) {
state.time_week = temp.WNc;
state.time_week_ms = (uint32_t)(temp.TOW);
}
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = last_hdop;
// Update velocity state (don't use −2·10^10)
if (temp.Vn > -200000) {
state.velocity.x = (float)(temp.Vn);
state.velocity.y = (float)(temp.Ve);
state.velocity.z = (float)(-temp.Vu);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.horizontal_accuracy = (float)temp.HAccuracy * 0.01f;
state.vertical_accuracy = (float)temp.VAccuracy * 0.01f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
// Update position state (don't use −2·10^10)
if (temp.Latitude > -200000) {
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * 1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * 1e7);
state.location.alt = (int32_t)((float)temp.Height * 1e2f);
}
if (temp.NrSV != 255) {
state.num_sats = temp.NrSV;
}
Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode);
switch (temp.Mode & 15) {
case 0: // no pvt
state.status = AP_GPS::NO_FIX;
break;
case 1: // standalone
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // dgps
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // fixed location
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
break;
case 5: // rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 6: // sbas
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 7: // moving rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
break;
case 8: // moving rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
}
if ((temp.Mode & 64) > 0) // gps is in base mode
state.status = AP_GPS::NO_FIX;
if ((temp.Mode & 128) > 0) // gps only has 2d fix
state.status = AP_GPS::GPS_OK_FIX_2D;
return true;
}
// DOP
if (blockid == 4001) {
const msg4001 &temp = sbf_msg.data.msg4001u;
last_hdop = temp.HDOP;
state.hdop = last_hdop;
}
return false;
}
bool
AP_GPS_SBP2::_attempt_state_update()
{
if (last_heartbeat_received_ms == 0)
return false;
uint32_t now = AP_HAL::millis();
if (now - last_heartbeat_received_ms > SBP_TIMEOUT_HEARTBEAT) {
state.status = AP_GPS::NO_FIX;
Info("No Heartbeats from Piksi! Status to NO_FIX.");
return false;
} else if (last_heartbeat.protocol_major != 2) {
state.status = AP_GPS::NO_FIX;
Info("Received a heartbeat from non-SBPv2 device. Current driver only supports SBPv2. Status to NO_FIX.");
return false;
} else if (last_heartbeat.nap_error == 1 ||
last_heartbeat.io_error == 1 ||
last_heartbeat.sys_error == 1) {
state.status = AP_GPS::NO_FIX;
Info("Piksi reported an error. Status to NO_FIX.");
Debug(" ext_antenna: %d", last_heartbeat.ext_antenna);
Debug(" res2: %d", last_heartbeat.res2);
Debug(" protocol_major: %d", last_heartbeat.protocol_major);
Debug(" protocol_minor: %d", last_heartbeat.protocol_minor);
Debug(" res: %d", last_heartbeat.res);
Debug(" nap_error: %d", last_heartbeat.nap_error);
Debug(" io_error: %d", last_heartbeat.io_error);
Debug(" sys_error: %d", last_heartbeat.sys_error);
return false;
} else if (last_pos_llh.tow == last_vel_ned.tow
&& (distMod(last_gps_time.tow, last_vel_ned.tow, AP_MSEC_PER_WEEK) < 10000)
&& (distMod(last_dops.tow, last_vel_ned.tow, AP_MSEC_PER_WEEK) < 60000)
&& (last_vel_ned.tow > last_full_update_tow || (last_gps_time.wn > last_full_update_wn && last_vel_ned.tow < last_full_update_tow))) {
//We have an aligned VEL and LLH, and a recent DOPS and TIME.
//
// Check Flags for Valid Messages
//
if (last_gps_time.flags.fix_mode == 0 ||
last_vel_ned.flags.fix_mode == 0 ||
last_pos_llh.flags.fix_mode == 0 ||
last_dops.flags.fix_mode == 0) {
Debug("Message Marked as Invalid. NO FIX! Flags: {GPS_TIME: %d, VEL_NED: %d, POS_LLH: %d, DOPS: %d}",
last_gps_time.flags.fix_mode,
last_vel_ned.flags.fix_mode,
last_pos_llh.flags.fix_mode,
last_dops.flags.fix_mode);
state.status = AP_GPS::NO_FIX;
return false;
}
//
// Update external time and accuracy state
//
state.time_week = last_gps_time.wn;
state.time_week_ms = last_vel_ned.tow;
state.hdop = last_dops.hdop;
state.vdop = last_dops.vdop;
state.last_gps_time_ms = now;
//
// Update velocity state
//
state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.speed_accuracy = safe_sqrt(
powf((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) +
powf((float)last_vel_ned.v_accuracy * 1.0e-3f, 2));
state.horizontal_accuracy = (float) last_pos_llh.h_accuracy * 1.0e-3f;
state.vertical_accuracy = (float) last_pos_llh.v_accuracy * 1.0e-3f;
//
// Set flags appropriately
//
state.have_vertical_velocity = true;
state.have_speed_accuracy = !is_zero(state.speed_accuracy);
state.have_horizontal_accuracy = !is_zero(state.horizontal_accuracy);
state.have_vertical_accuracy = !is_zero(state.vertical_accuracy);
//
// Update position state
//
state.location.lat = (int32_t) (last_pos_llh.lat * (double)1e7);
state.location.lng = (int32_t) (last_pos_llh.lon * (double)1e7);
state.location.alt = (int32_t) (last_pos_llh.height * 100);
state.num_sats = last_pos_llh.n_sats;
switch (last_pos_llh.flags.fix_mode) {
case 1:
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2:
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3:
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 4:
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 6:
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
default:
state.status = AP_GPS::NO_FIX;
break;
}
//
// Update Internal Timing
//
last_full_update_tow = last_vel_ned.tow;
last_full_update_wn = last_gps_time.wn;
return true;
}
return false;
}
/// advance_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_target_along_track(float dt)
{
float track_covered;
Vector3f track_error;
float track_desired_max;
float track_desired_temp = _track_desired;
float track_extra_max;
// get current location
Vector3f curr_pos = _inav->get_position();
Vector3f curr_delta = curr_pos - _origin;
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
Vector3f track_covered_pos = _pos_delta_unit * track_covered;
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
float track_error_xy = safe_sqrt(track_error.x*track_error.x + track_error.y*track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
track_extra_max = min(_track_leash_length*(_wp_leash_z-track_error_z)/_wp_leash_z, _track_leash_length*(_wp_leash_xy-track_error_xy)/_wp_leash_xy);
if(track_extra_max <0) {
track_desired_max = track_covered;
}else{
track_desired_max = track_covered + track_extra_max;
}
// get current velocity
Vector3f curr_vel = _inav->get_velocity();
// get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
// calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms;
float kP = _pid_pos_lat->kP();
if (kP >= 0.0f) { // avoid divide by zero
linear_velocity = _track_accel/kP;
}
// let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity) {
// we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
_limited_speed_xy_cms = 0;
}else{
// increase intermediate target point's velocity if not yet at target speed (we will limit it below)
if(dt > 0) {
if(track_desired_max > _track_desired) {
_limited_speed_xy_cms += 2.0 * _track_accel * dt;
}else{
// do nothing, velocity stays constant
_track_desired = track_desired_max;
}
}
// do not go over top speed
if(_limited_speed_xy_cms > _track_speed) {
_limited_speed_xy_cms = _track_speed;
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
}
}
// advance the current target
track_desired_temp += _limited_speed_xy_cms * dt;
// do not let desired point go past the end of the segment
track_desired_temp = constrain_float(track_desired_temp, 0, _track_length);
_track_desired = max(_track_desired, track_desired_temp);
// recalculate the desired position
_target = _origin + _pos_delta_unit * _track_desired;
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
if( _track_desired >= _track_length ) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint) {
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = curr_pos - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}
}
bool
AP_GPS_SBP::_attempt_state_update()
{
// If we currently have heartbeats
// - NO FIX
//
// If we have a full update available, save it
//
uint32_t now = AP_HAL::millis();
bool ret = false;
if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
state.status = AP_GPS::NO_FIX;
Debug("No Heartbeats from Piksi! Driver Ready to Die!");
} else if (last_pos_llh_rtk.tow == last_vel_ned.tow
&& abs((int32_t) (last_gps_time.tow - last_vel_ned.tow)) < 10000
&& abs((int32_t) (last_dops.tow - last_vel_ned.tow)) < 60000
&& last_vel_ned.tow > last_full_update_tow) {
// Use the RTK position
sbp_pos_llh_t *pos_llh = &last_pos_llh_rtk;
// Update time state
state.time_week = last_gps_time.wn;
state.time_week_ms = last_vel_ned.tow;
state.hdop = last_dops.hdop;
// Update velocity state
state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
// Update position state
state.location.lat = (int32_t) (pos_llh->lat * (double)1e7);
state.location.lng = (int32_t) (pos_llh->lon * (double)1e7);
state.location.alt = (int32_t) (pos_llh->height * 100);
state.num_sats = pos_llh->n_sats;
if (pos_llh->flags == 0) {
state.status = AP_GPS::GPS_OK_FIX_3D;
} else if (pos_llh->flags == 2) {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
} else if (pos_llh->flags == 1) {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
}
last_full_update_tow = last_vel_ned.tow;
last_full_update_cpu_ms = now;
logging_log_full_update();
ret = true;
} else if (now - last_full_update_cpu_ms > SBP_TIMEOUT_PVT) {
//INVARIANT: If we currently have a fix, ONLY return true after a full update.
state.status = AP_GPS::NO_FIX;
ret = true;
} else {
//No timeouts yet, no data yet, nothing has happened.
}
return ret;
}
/// Calculates the magnitude of a quaternion
REAL QUAT::magnitude() const
{
return safe_sqrt(w*w + x*x + y*y + z*z);
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
_pos_delta_unit = target_vel/target_vel.length();
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cms * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
break;
}
uint32_t tnow = AP_HAL::millis();
mavlink_rc_channels_override_t packet;
mavlink_msg_rc_channels_override_decode(msg, &packet);
RC_Channels::set_override(0, packet.chan1_raw, tnow);
RC_Channels::set_override(1, packet.chan2_raw, tnow);
RC_Channels::set_override(2, packet.chan3_raw, tnow);
RC_Channels::set_override(3, packet.chan4_raw, tnow);
RC_Channels::set_override(4, packet.chan5_raw, tnow);
RC_Channels::set_override(5, packet.chan6_raw, tnow);
RC_Channels::set_override(6, packet.chan7_raw, tnow);
RC_Channels::set_override(7, packet.chan8_raw, tnow);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
break;
}
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
if (packet.target != rover.g.sysid_this_mav) {
break; // only accept control aimed at us
}
uint32_t tnow = AP_HAL::millis();
const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f;
const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f;
RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow);
RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if (msg->sysid != rover.g.sysid_my_gcs) {
break;
}
rover.last_heartbeat_ms = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
break;
}
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// ensure type_mask specifies to use thrust
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
break;
}
// convert thrust to ground speed
packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
// if the body_yaw_rate field is ignored, convert quaternion to heading
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
// convert quaternion to heading
float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
} else {
// use body_yaw_rate field
rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
switch (packet.coordinate_frame) {
case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_NED: {
// rotate from body-frame to NE frame
const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
// add offset to current location
location_offset(target_loc, ne_x, ne_y);
}
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location
location_offset(target_loc, packet.x, packet.y);
break;
default:
// MAV_FRAME_LOCAL_NED interpret as an offset from home
target_loc = rover.ahrs.get_home();
location_offset(target_loc, packet.x, packet.y);
break;
}
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw heading
if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != &rover.mode_guided) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
// prepare target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
// sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) {
// result = MAV_RESULT_FAILED;
break;
}
target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_yaw_cd = 0.0f;
// consume velocity and convert to target speed and heading
if (!vel_ignore) {
const float speed_max = rover.control_mode->get_speed_default();
// convert vector length into a speed
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
// convert vector direction to target yaw
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw heading
if (!yaw_ignore) {
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
// rotate target yaw if provided in body-frame
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
}
}
// consume yaw rate
float target_turn_rate_cds = 0.0f;
if (!yaw_rate_ignore) {
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// handling case when both velocity and either yaw or yaw-rate are provided
// by default, we consider that the rover will drive forward
float speed_dir = 1.0f;
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
// Note: we are using the x-axis velocity to determine direction even though
// the frame is provided in MAV_FRAME_GLOBAL_xxx
if (is_negative(packet.vx)) {
speed_dir = -1.0f;
}
}
// set guided mode targets
if (!pos_ignore) {
// consume position target
rover.mode_guided.set_desired_location(target_loc);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume velocity and turn rate
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume velocity
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
// consume just target heading (probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
// consume just turn rate(probably only skid steering vehicles can do this)
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
}
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE
#if MOUNT == ENABLED
// deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
rover.camera_mount.configure_msg(msg);
break;
}
// deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
rover.camera_mount.control_msg(msg);
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
break;
}
// send or receive fence points with GCS
case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160
case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
rover.g2.fence.handle_msg(*this, msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
rover.rangefinder.handle_msg(msg);
rover.g2.proximity.handle_msg(msg);
break;
default:
handle_common_message(msg);
break;
} // end switch
} // end handle mavlink
bool apm::AP_GPS_SBP::_attempt_state_update()
{
// If we currently have heartbeats
// - NO FIX
//
// If we have a full update available, save it
//
uint32_t now = quan::stm32::millis().numeric_value();
bool ret = false;
if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
gps.state.status = gps_t::NO_GPS;
// Debug("No Heartbeats from Piksi! Driver Ready to Die!");
ret = false;
} else if (last_pos_llh_rtk.tow == last_vel_ned.tow
&& abs((int32_t) (last_gps_time.tow - last_vel_ned.tow)) < 10000
&& abs((int32_t) (last_dops.tow - last_vel_ned.tow)) < 60000
&& last_vel_ned.tow > last_full_update_tow) {
// Use the RTK position
sbp_pos_llh_t *pos_llh = &last_pos_llh_rtk;
// Update time state
gps.state.time_week = last_gps_time.wn;
gps.state.time_week_ms = last_vel_ned.tow;
gps.state.hdop = last_dops.hdop;
// Update velocity state
gps.state.velocity[0] = gps_t::velocity_type{last_vel_ned.n / 1000.0};
gps.state.velocity[1] = gps_t::velocity_type{last_vel_ned.e / 1000.0};
gps.state.velocity[2] = gps_t::velocity_type{last_vel_ned.d / 1000.0};
float ground_vector_sq = gps.state.velocity[0].numeric_value()*gps.state.velocity[0].numeric_value()
+ gps.state.velocity[1].numeric_value()*gps.state.velocity[1].numeric_value();
gps.state.ground_speed = safe_sqrt(ground_vector_sq);
gps.state.ground_course_cd = wrap_360_cd((int32_t) 100*ToDeg(atan2f(gps.state.velocity[1].numeric_value(), gps.state.velocity[0].numeric_value())));
// Update position state
gps.state.location.lat = gps_t::lat_lon_type{pos_llh->lat*1e7};
gps.state.location.lon = gps_t::lat_lon_type{pos_llh->lon*1e7};
gps.state.location.alt = gps_t::altitude_type{pos_llh->height*1e2};
gps.state.num_sats = pos_llh->n_sats;
switch (pos_llh->flags){
case 0:
gps.state.status = gps_t::FIX_3D;
break;
case 2:
gps.state.status = gps_t::FIX_3D_DGPS;
break;
case 1:
gps.state.status = gps_t::FIX_3D_RTK;
break;
default:
break;
}
last_full_update_tow = last_vel_ned.tow;
last_full_update_cpu_ms = now;
// logging_log_full_update();
ret = true;
} else if (now - last_full_update_cpu_ms > SBP_TIMEOUT_PVT) {
//INVARIANT: If we currently have a fix, ONLY return true after a full update.
gps.state.status = gps_t::NO_FIX;
ret = true;
} else {
//No timeouts yet, no data yet, nothing has happened.
ret = false;
}
return ret;
}
