fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 03:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics

Browse Source 
MC/FW rate controller and auto tuner: remove actuator_controls

AirshipAttControl: remove actuator_controls

MulticopterLandDetector: remove actuator_controls

mavlink streams vfr_hud and high_latency2: remove actuator_controls

RoverPositionController: remove actuator_controls

UUVAttitudeController: remove actuator_controls

battery: use length of thrust_setpoint for throttle compensation

VehicleMagnetometer: use length of thrust_setpoint for throttle compensation

Signed-off-by: Silvan Fuhrer
This commit is contained in:
Silvan Fuhrer
2022-12-23 12:57:18 +01:00
committed by Beat Küng
parent bcd6e7adee
commit b16f16598b
47 changed files with 332 additions and 549 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_pos_control/RoverPositionControl.cpp
+17 -23
View File
@@ -149,13 +149,10 @@ RoverPositionControl::manual_control_setpoint_poll()
_attitude_sp_pub.publish(_att_sp);
} else {
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch;
// Set heading from the manual roll input channel
_act_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
_yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw;
// Set throttle from the manual throttle channel
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_throttle_control = (_manual_control_setpoint.throttle + 1.f) * .5f;
_reset_yaw_sp = true;
}
@@ -276,21 +273,21 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
prev_wp(1));
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
_throttle_control = mission_throttle;
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;
_yaw_control = control_effort;
}
}
break;
case STOPPING: {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_yaw_control = 0.0f;
_throttle_control = 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));
@@ -336,7 +333,7 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt);
//Constrain maximum throttle to mission throttle
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, mission_throttle);
_throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle);
Vector3f desired_body_velocity;
@@ -352,12 +349,12 @@ RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
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;
_yaw_control = control_effort;
} else {
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_throttle_control = 0.0f;
_yaw_control = 0.0f;
}
}
@@ -371,11 +368,11 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
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;
_yaw_control = 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);
_throttle_control = math::constrain(control_throttle, 0.0f, 1.0f);
}
@@ -481,22 +478,19 @@ RoverPositionControl::Run()
_control_mode.flag_control_attitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_manual_enabled) {
// timestamp and publish controls
_act_controls.timestamp = hrt_absolute_time();
_actuator_controls_pub.publish(_act_controls);
vehicle_thrust_setpoint_s v_thrust_sp{};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[0] = _throttle_control;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
vehicle_torque_setpoint_s v_torque_sp{};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.xyz[0] = _act_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW];
v_torque_sp.xyz[0] = 0.f;
v_torque_sp.xyz[1] = 0.f;
v_torque_sp.xyz[2] = _yaw_control;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
}
@@ -541,7 +535,7 @@ int RoverPositionControl::print_usage(const char *reason)
### Description
Controls the position of a ground rover using an L1 controller.
Publishes `actuator_controls_0` messages at IMU_GYRO_RATEMAX.
Publishes `vehicle_thrust_setpoint (only in x) and vehicle_torque_setpoint (only yaw)` messages at IMU_GYRO_RATEMAX.
### Implementation
Currently, this implementation supports only a few modes: