Lqs66
June 10, 2023, 5:55am
1
I’ve recently been learning the source code for px4’s flight mode, and I was wondering where the source code for the stabilized mode control is.
Lqs66
June 10, 2023, 3:48pm
2
Like altitude mode, position mode has its own control logic code, I think stabilized mode also has its own control logic code, where should I look for it?
Good question. It’s not obvious because it’s not a task in FlightTasks.
This is where Acro is mapped:
if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) {
// generate the rate setpoint from sticks
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// manual rates control - ACRO mode
const Vector3f man_rate_sp{
math::superexpo(manual_control_setpoint.roll, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(-manual_control_setpoint.pitch, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
math::superexpo(manual_control_setpoint.yaw, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
_thrust_setpoint(2) = -(manual_control_setpoint.throttle + 1.f) * .5f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
// publish rate setpoint
vehicle_rates_setpoint.roll = _rates_setpoint(0);
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
show original
And this is stabilized, I think:
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_vehicle_control_mode.flag_control_manual_enabled &&
!_vehicle_control_mode.flag_control_altitude_enabled &&
!_vehicle_control_mode.flag_control_velocity_enabled &&
!_vehicle_control_mode.flag_control_position_enabled) {
generate_attitude_setpoint(q, dt, _reset_yaw_sp);
attitude_setpoint_generated = true;
} else {
_man_roll_input_filter.reset(0.f);
_man_pitch_input_filter.reset(0.f);
}
Vector3f rates_sp = _attitude_control.update(q);
1 Like
Lqs66
June 11, 2023, 6:38am
4
Thanks for your help, I found it.
1 Like
