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