fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 01:33:33 +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
2023-03-16 11:55:45 +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:
src/modules/rover_pos_control/RoverPositionControl.hpp
+2 -3
View File
@@ -72,7 +72,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
@@ -108,7 +107,6 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
@@ -125,7 +123,6 @@ private:
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
trajectory_setpoint_s _trajectory_setpoint{};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
@@ -170,6 +167,8 @@ private:
float _manual_yaw_sp{0.0};
bool _reset_yaw_sp{true};
float _throttle_control{0.f};
float _yaw_control{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,