forked from Archive/PX4-Autopilot
Turned all control outputs into NED frame moments, this is validated in real flight with a correct mixer setup.
This commit is contained in:
parent
f57b7fe0dd
commit
d129eff5b9
|
@ -322,11 +322,24 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
_att.roll, _att.pitch, _att.yaw,
|
_att.roll, _att.pitch, _att.yaw,
|
||||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed
|
||||||
);
|
);
|
||||||
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
|
_actuators.control[CH_AIL] = - _backsideAutopilot.getAileron();
|
||||||
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
|
_actuators.control[CH_ELV] = - _backsideAutopilot.getElevator();
|
||||||
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
|
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
|
||||||
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
|
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
|
||||||
|
|
||||||
|
// XXX limit throttle to manual setting (safety) for now.
|
||||||
|
// If it turns out to be confusing, it can be removed later once
|
||||||
|
// a first binary release can be targeted.
|
||||||
|
// This is not a hack, but a design choice.
|
||||||
|
|
||||||
|
/* do not limit in HIL */
|
||||||
|
if (!_status.flag_hil_enabled) {
|
||||||
|
/* limit to value of manual throttle */
|
||||||
|
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
|
||||||
|
_actuators.control[CH_THR] : _manual.throttle;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
|
|
||||||
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||||
|
@ -337,11 +350,12 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
_actuators.control[CH_THR] = _manual.throttle;
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
|
|
||||||
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||||
_stabilization.update(
|
|
||||||
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
|
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
|
||||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||||
|
|
||||||
_actuators.control[CH_AIL] = _stabilization.getAileron();
|
_actuators.control[CH_AIL] = _stabilization.getAileron();
|
||||||
_actuators.control[CH_ELV] = _stabilization.getElevator();
|
_actuators.control[CH_ELV] = - _stabilization.getElevator();
|
||||||
_actuators.control[CH_RDR] = _stabilization.getRudder();
|
_actuators.control[CH_RDR] = _stabilization.getRudder();
|
||||||
_actuators.control[CH_THR] = _manual.throttle;
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
}
|
}
|
||||||
|
|
|
@ -196,9 +196,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||||
/* roll rate (PI) */
|
/* roll rate (PI) */
|
||||||
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
|
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
|
||||||
/* pitch rate (PI) */
|
/* pitch rate (PI) */
|
||||||
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
|
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
|
||||||
/* set pitch minus feedforward throttle compensation (nose pitches up from throttle */
|
|
||||||
actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust;
|
|
||||||
/* yaw rate (PI) */
|
/* yaw rate (PI) */
|
||||||
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
|
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue