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(0), ground_speed(1)};
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 = 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.nav_roll();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust = 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.nav_roll();
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
_att_sp.thrust = 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 = 0.0f;
/* do not publish the setpoint */
setpoint = false;
}
return setpoint;
}
