PX4从放弃到精通（二十）：无人车(船)

文章目录

• ​​前言​​
• ​​一、主程序​​
• ​​二、位置控制​​
• ​​三、速度控制​​
• ​​四、姿态控制​​
• ​​五、L1导航​​

前言

PX4固件版本:1.11.3

一、主程序

程序位置

订阅需要的数据并设置订阅的频率

void
RoverPositionControl::run()
{
  _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
  _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
  _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
  _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));

  _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
  _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));

  /* rate limit control mode updates to 5Hz */
  orb_set_interval(_control_mode_sub, 200);

  /* rate limit position updates to 50 Hz */
  orb_set_interval(_global_pos_sub, 20);
  orb_set_interval(_local_pos_sub, 20);

更新参数

parameters_update(true);

阻塞等待

/* wakeup source(s) */
  px4_pollfd_struct_t fds[5];

  /* Setup of loop */
  fds[0].fd = _global_pos_sub;
  fds[0].events = POLLIN;
  fds[1].fd = _manual_control_setpoint_sub;
  fds[1].events = POLLIN;
  fds[2].fd = _sensor_combined_sub;
  fds[2].events = POLLIN;
  fds[3].fd = _vehicle_attitude_sub; // Poll attitude
  fds[3].events = POLLIN;
  fds[4].fd = _local_pos_sub;  // Added local position as source of position
  fds[4].events = POLLIN;

大循环

while (!should_exit()) {

等待数据

/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
  /* this is undesirable but not much we can do - might want to flag unhappy status */

没有数据则不执行下面的语句，重新循环

if (pret < 0) {
  warn("poll error %d, %d", pret, errno);
  continue;
}

如果消息更新，则订阅消息

具体定义如下：

/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
attitude_setpoint_poll();

订阅加速度

_vehicle_acceleration_sub.update();

跟新参数

/* update parameters from storage */
parameters_update();

手动模式标志

bool manual_mode = _control_mode.flag_control_manual_enabled;

如果全球位置和本地位置跟新，则运行控制器

/* only run controller if position changed */
if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) {
  perf_begin(_loop_perf);

订阅全球位置和本地位置

/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
position_setpoint_triplet_poll();

如果是offboard模式

//Convert Local setpoints to global setpoints
if (_control_mode.flag_control_offboard_enabled) {

如果没有初始化本地坐标原点经纬度，则初始化本地坐标原点经纬度

if (!globallocalconverter_initialized()) {
  globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon,
          _local_pos.ref_alt, _local_pos.ref_timestamp);

}

将全球坐标转为本地坐标

else {
          globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z,
                      &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt);

        }
      }

重置计数

// update the reset counters in any case
_pos_reset_counter = _global_pos.lat_lon_reset_counter;

定义位置和速度向量

matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy,  _local_pos.vz);
matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);

如果是位置控制模式

if (!manual_mode && _control_mode.flag_control_position_enabled) {

运行位置控制器

if (control_position(current_position, ground_speed, _pos_sp_triplet)) {

赋值相应的状态并发布

//TODO: check if radius makes sense here
  float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f);

  // publish status
  position_controller_status_s pos_ctrl_status = {};

  pos_ctrl_status.nav_roll = 0.0f;
  pos_ctrl_status.nav_pitch = 0.0f;
  pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();

  pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
  pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();

  pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
          _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);

  pos_ctrl_status.acceptance_radius = turn_distance;
  pos_ctrl_status.yaw_acceptance = NAN;

  pos_ctrl_status.timestamp = hrt_absolute_time();

  _pos_ctrl_status_pub.publish(pos_ctrl_status);


}

如果是速度控制模式，则运行速度控制器

} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) {
  control_velocity(current_velocity, _pos_sp_triplet);
}
      perf_end(_loop_perf);
    }

如果姿态更新并且是使能了姿态控制，则运行姿态控制器

if (fds[3].revents & POLLIN) {

  vehicle_attitude_poll();

  if (!manual_mode && _control_mode.flag_control_attitude_enabled
      && !_control_mode.flag_control_position_enabled
      && !_control_mode.flag_control_velocity_enabled) {

    control_attitude(_vehicle_att, _att_sp);

  }

}

订阅遥控器值

if (fds[1].revents & POLLIN) {

  // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
  // returning immediately and this loop will eat up resources.
  orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);

如果是遥控模式，则直接根据遥控器输出，这里虽然赋值四个，但只有偏航和推力有效

if (manual_mode) {
    /* manual/direct control */
    //PX4_INFO("Manual mode!");
    _act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
    _act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
    _act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param
    _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
  }
}

订阅传感器数据

if (fds[2].revents & POLLIN) {

  orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

只要有任何一种控制方式使能，就发布控制量

//orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
    _act_controls.timestamp = hrt_absolute_time();

    /* Only publish if any of the proper modes are enabled */
    if (_control_mode.flag_control_velocity_enabled ||
        _control_mode.flag_control_attitude_enabled ||
        _control_mode.flag_control_position_enabled ||
        manual_mode) {
      /* publish the actuator controls */
      _actuator_controls_pub.publish(_act_controls);
    }
  }

}

取消订阅

orb_unsubscribe(_control_mode_sub);
  orb_unsubscribe(_global_pos_sub);
  orb_unsubscribe(_local_pos_sub);
  orb_unsubscribe(_manual_control_setpoint_sub);
  orb_unsubscribe(_pos_sp_triplet_sub);
  orb_unsubscribe(_vehicle_attitude_sub);
  orb_unsubscribe(_sensor_combined_sub);

  warnx("exiting.\n");
}

二、位置控制

bool
RoverPositionControl::control_position(const matrix::Vector2f &current_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

更新dt

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;

如果是auto或者offboard模式，并且存在期望位置

if ((_control_mode.flag_control_auto_enabled ||
     _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) {

赋值控制模式

_control_mode_current = UGV_POSCTRL_MODE_AUTO;

期望经纬度航点向量

/* current waypoint (the one currently heading for) */
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);

如果存在上一个航点，则更新上一个航点，运行L1算法必须知道下一个航点和上一个航点

/* 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 = _param_throttle_cruise.get();

/* Just control the throttle */
  if (_param_speed_control_mode.get() == 1) {
    /* control the speed in closed loop */

获取巡航速度

float mission_target_speed = _param_gndspeed_trim.get();

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(_vehicle_att.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 = _vehicle_acceleration_sub.get().xyz[0];

PID计算期望推力

// Compute airspeed control out and just scale it as a constant
mission_throttle = _param_throttle_speed_scaler.get()
       * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);

推力限幅

// Constrain throttle between min and max
mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get());

如果在​​_pos_sp_triplet​​中指定了期望推力，则使用开换控制

} 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;
  }
}

获取当前位置到下一个航点的距离

float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
        (double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);

如果是跑航点

switch (_pos_ctrl_state) {
case GOTO_WAYPOINT: {

如果当前点到下一个航点的距离小于参数​​NAV_LOITER_RAD​​，则进入停止

if (dist_target < _param_nav_loiter_rad.get()) {
  _pos_ctrl_state = STOPPING;  // We are closer than loiter radius to waypoint, stop.

}

利用L1算法计算侧向加速度

else {
          _gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);

赋值推力

_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

根据侧向加速度计算偏航力矩．无人车/船在施加偏航力矩后，实际上是在做圆周运动，根据圆周运动加速度计算公式：​​a=v^2/r​​

其中期望加速度​​a​​和运动速度​​v​​已知，可以求出期望的转弯半径​​r＝v^2/a​​,这里只计算绝对值．

float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());

根据几何知识，可以由转弯半径和轮子直径计算转舵的角度

float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());

根据转舵角度结合侧向加速度的方向，计算出偏航控制量大小和方向

float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
　_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);

输出

_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
  }
}
  break;

停止状态偏航和推力都为0，

case STOPPING: {
    _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
    _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;

求上一个航点与下一个航点的距离

// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
             (double)curr_wp(0), (double)curr_wp(1));

如果距离大于0,说明有新的航点，进入航点状态

if (dist_between_waypoints > 0) {
  _pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
}
      }
      break;

默认置为停止状态

default:
  PX4_ERR("Unknown Rover State");
  _pos_ctrl_state = STOPPING;
  break;
}

当前航点置为上一次的航点，

_prev_wp = curr_wp;

} else {
    _control_mode_current = UGV_POSCTRL_MODE_OTHER;
    setpoint = false;
  }

  return setpoint;
}

三、速度控制

void
RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
               const position_setpoint_triplet_s &pos_sp_triplet)
{

控制器的运行时间间隔，使用非零值避免被零除

float dt = 0.01; // Using non zero value to a avoid division by zero

定义期望推力

const float mission_throttle = _param_throttle_cruise.get();

从​​pos_sp_triplet​​中取期望速度

const matrix::Vector3f desired_velocity{pos_sp_triplet.current.vx, pos_sp_triplet.current.vy, pos_sp_triplet.current.vz};

期望速度取模

const float desired_speed = desired_velocity.norm();

将速度转换到机体坐标系

if (desired_speed > 0.01f) {
  const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
  const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2));

定义机头方向的速度和家速度

const float x_vel = vel(0);
const float x_acc = _vehicle_acceleration_sub.get().xyz[0];

PID计算推力

const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);

限幅并赋值，幅度从0到​​mission_throttle​​

//Constrain maximum throttle to mission throttle
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, mission_throttle);

定义期望的机体速度

Vector3f desired_body_velocity;

如果期望的速度是机体坐标系的，则直接使用，如果是地理坐标系，则转换为机体坐标系下

if (pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
  desired_body_velocity = desired_velocity;
} 
else {
      // If the frame of the velocity setpoint is unknown, assume it is in local frame
      desired_body_velocity = R_to_body * desired_velocity;
    }

求机体坐标系下期望速度的角度。

const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0));

除以参数​​GND_MAX_ANG​​，该参数越小，偏航角的控制越灵敏，如果船走S形，将该参数调小

float control_effort = desired_theta / _param_max_turn_angle.get();

赋值偏航控制量

control_effort = math::constrain(control_effort, -1.0f, 1.0f);

  _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;

}

else {

    _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
    _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;

  }
}

四、姿态控制

void
RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
{

计算偏航角误差

// quaternion attitude control law, qe is rotation from q to qd
const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
const Eulerf euler_sp = qe;

将偏航角误差除以​​GND_MAX_ANG​​直接赋值

float control_effort = euler_sp(2) / _param_max_turn_angle.get();
control_effort = math::constrain(control_effort, -1.0f, 1.0f);

_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;

根据设定值赋值推力

const float control_throttle = att_sp.thrust_body[0];
  _act_controls.control[actuator_controls_s::INDEX_THROTTLE] =  math::constrain(control_throttle, 0.0f, 1.0f);

}

五、L1导航

L1导航控制的作用是计算控制无人船跑航线时的侧向加速度

代码位置：

​​vector_A​​​是上一个航点的位置（刚经过的那个），​​vector_B​​​是现在正要去的那个航点的位置，​​vector_curr_position​​​是当前位置，​​ground_speed_vector​​是速度

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));

限制速度最小为0.1，防止除0.

/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);

计算​​_L1_distance​​，

/* calculate the L1 length required for the desired period */
_L1_distance = _L1_ratio * ground_speed;

其中​​_L1_ratio​​由下式求得

​​_L1_damping​​和​​_L1_period​​对应下面的参数

计算由向量A指向向量B的向量

/* 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();

计算有A点指向当前位置的向量

/* 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);

L1算法的原理如下

下面分三种情况求​​eta​​

/* 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) {

将A点作为参考点，如下图

叉乘求​​sin（eta）*ground_speed_vector的模​​，再点乘求​​cos（eta）*ground_speed_vector的模​​，再相除求​​tan（eta）​​，最后反正切得角度。

/* 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)) {

直接把B点作为参考点，如下图

叉乘求​​sin（eta）*ground_speed_vector的模​​，再点乘求​​cos（eta）*ground_speed_vector的模​​，再相除求​​tan（eta）​​，最后反正切得角度。

/*
   * 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 {

利用标准的L1算法求​​eta​​，如下图。

​​eta2​​​就是求速度与航线方向的夹脚，先叉乘求​​sin（eta2）*ground_speed_vector的模​​​，再点乘求​​cos（eta2）*ground_speed_vector的模​​​，再相除求​​tan（eta2）​​，最后反正切得角度。

/* 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) */

这个​​xtrackErr​​​和上面图中的​​_crosstrack_error​​是一样的，

float xtrackErr = vector_A_to_airplane % vector_AB;

如图中，​​xtrackErr​​​/​​_L1_distance​​​得到​​eta1​​的sin值

float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f);

限幅并求​​eta1​​

/* 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​​

eta = eta1 + eta2;
  /* bearing from current position to L1 point */
  _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
}

限幅​​eta​​并利用L1算法公式求侧向加速度

/* 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();
}