交流学习加qq:2096723956
gnd_att_control_params.c和gnd_pos_control_params.c两个文件里存放的是相关的参数
GroundRoverAttitudeControl.cpp和GroundRoverAttitudeControl.hpp存放的是姿态控制的代码
GroundRoverPositionControl.cpp和GroundRoverPositionControl.hpp存放的是位置控制的代码
ecl_l1_pos_controller.cpp和ecl_l1_pos_controller.h存放的是导航算法的代码
期望推力在GroundRoverPositionControl.cpp中通过pid得出
mission_throttle = _parameters.throttle_speed_scaler
* pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
_parameters.throttle_speed_scaler是固定的参数，_speed_ctrl是存放pid参数的结构体，mission_target_speed是期望的速度，x_vel是当前速度，x_acc是当前加速度，dt是时间间隔。
位置控制通过以下函数实现
bool
GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
{
float dt = 0.01; // Using non zero value to a avoid division by zero
if (_control_position_last_called > 0) {
dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
}
_control_position_last_called = hrt_absolute_time();
bool setpoint = true;
if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
_control_mode_current = UGV_POSCTRL_MODE_AUTO;
/* get circle mode */
bool was_circle_mode = _gnd_control.circle_mode();
/* current waypoint (the one currently heading for) */
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* previous waypoint */
matrix::Vector2f prev_wp = curr_wp;
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
}
matrix::Vector2f ground_speed_2d(ground_speed);
float mission_throttle = _parameters.throttle_cruise;
/* Just control the throttle */
if (_parameters.speed_control_mode == 1) {
/* control the speed in closed loop */
float mission_target_speed = _parameters.gndspeed_trim;
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
_pos_sp_triplet.current.cruising_speed > 0.1f) {
mission_target_speed = _pos_sp_triplet.current.cruising_speed;
}
// Velocity in body frame
const Dcmf R_to_body(Quatf(_sub_attitude.get().q).inversed());
const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
const float x_vel = vel(0);
const float x_acc = _sub_sensors.get().accel_x;
// Compute airspeed control out and just scale it as a constant
mission_throttle = _parameters.throttle_speed_scaler
* pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
// Constrain throttle between min and max
mission_throttle = math::constrain(mission_throttle, _parameters.throttle_min, _parameters.throttle_max);
} else {
/* Just control throttle in open loop */
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
_pos_sp_triplet.current.cruising_throttle > 0.01f) {
mission_throttle = _pos_sp_triplet.current.cruising_throttle;
}
}
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.thrust_body[0] = 0.0f;
} else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
|| (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
/* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _gnd_control.get_roll_setpoint();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = mission_throttle;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint so we want to stop*/
_gnd_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed_2d);
_att_sp.roll_body = _gnd_control.get_roll_setpoint();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = 0.0f;
}
if (was_circle_mode && !_gnd_control.circle_mode()) {
/* just kicked out of loiter, reset integrals */
_att_sp.yaw_reset_integral = true;
}
} else {
_control_mode_current = UGV_POSCTRL_MODE_OTHER;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = 0.0f;
/* do not publish the setpoint */
setpoint = false;
}
return setpoint;
}
current_position为当前位置，ground_speed为地速，pos_sp_triplet为期望位置，该函数主要计算期望的航向，横滚，推力
期望航向角在ecl_l1_pos_controller.cpp中通过以下函数得出
void
ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B,
const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector)
{
/* this follows the logic presented in [1] */
float eta = 0.0f;
float xtrack_vel = 0.0f;
float ltrack_vel = 0.0f;
/* get the direction between the last (visited) and next waypoint */
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), (double)vector_B(0), (double)vector_B(1));
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
/* calculate the L1 length required for the desired period */
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
* skip A and directly continue to B
*/
if (vector_AB.length() < 1.0e-6f) {
vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
}
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
/*
* If the current position is in a +-135 degree angle behind waypoint A
* and further away from A than the L1 distance, then A becomes the L1 point.
* If the aircraft is already between A and B normal L1 logic is applied.
*/
float distance_A_to_airplane = vector_A_to_airplane.length();
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
/* calculate angle of airplane position vector relative to line) */
// XXX this could probably also be based solely on the dot product
float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
/* extension from [2], fly directly to A */
if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) {
/* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */
Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
/*
* If the AB vector and the vector from B to airplane point in the same
* direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
*/
} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
/*
* Extension, fly back to waypoint.
*
* This corner case is possible if the system was following
* the AB line from waypoint A to waypoint B, then is
* switched to manual mode (or otherwise misses the waypoint)
* and behind the waypoint continues to follow the AB line.
*/
/* calculate eta to fly to waypoint B */
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));
} else {
/* calculate eta to fly along the line between A and B */
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % vector_AB;
/* velocity along line */
ltrack_vel = ground_speed_vector * vector_AB;
/* calculate eta2 (angle of velocity vector relative to line) */
float eta2 = atan2f(xtrack_vel, ltrack_vel);
/* calculate eta1 (angle to L1 point) */
float xtrackErr = vector_A_to_airplane % vector_AB;
float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);
/* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
}
/* limit angle to +-90 degrees */
eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
/* flying to waypoints, not circling them */
_circle_mode = false;
/* the bearing angle, in NED frame */
_bearing_error = eta;
update_roll_setpoint();
}
当前位置到下一航点的方向通过以下函数得出
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
const double lat_now_rad = math::radians(lat_now);
const double lat_next_rad = math::radians(lat_next);
const double cos_lat_next = cos(lat_next_rad);
const double d_lon = math::radians(lon_next - lon_now);
/* conscious mix of double and float trig function to maximize speed and efficiency */
const float y = static_cast<float>(sin(d_lon) * cos_lat_next);
const float x = static_cast<float>(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos_lat_next * cos(d_lon));
return wrap_pi(atan2f(y, x));
}
其中radians是将角度转化为弧度。wrap_pi是将（x,y）向量的方向计算出来。
