Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

fw_att_control consolidate flaps code

Browse Source
This commit is contained in:
Daniel Agar
2018-02-17 10:38:18 -05:00
parent 4cb71f209f
commit a0f115b9ae
2 changed files with 48 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+46 -41
View File
@@ -550,47 +550,7 @@ void FixedwingAttitudeControl::run()
continue; continue;
} }
/* default flaps to center */ control_flaps(deltaT);
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? deltaT : -deltaT;
} else {
_flaps_applied = flap_control;
}
/* default flaperon to center */
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? deltaT : -deltaT;
} else {
_flaperons_applied = flaperon_control;
}
/* decide if in stabilized or full manual control */ /* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) { if (_vcontrol_mode.flag_control_rates_enabled) {
@@ -859,6 +819,51 @@ void FixedwingAttitudeControl::run()
} }
} }
void FixedwingAttitudeControl::control_flaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaps_scale) > 0.01f) {
flap_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
} else {
_flaps_applied = flap_control;
}
/* default flaperon to center */
float flaperon_control = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_parameters.flaperon_scale) > 0.01f) {
flaperon_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
}
FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[]) FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[])
{ {
return new FixedwingAttitudeControl(); return new FixedwingAttitudeControl();
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2 -23
View File
@@ -267,39 +267,18 @@ private:
ECL_YawController _yaw_ctrl; ECL_YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl; ECL_WheelController _wheel_ctrl;
void control_flaps(const float dt);
/** /**
* Update our local parameter cache. * Update our local parameter cache.
*/ */
int parameters_update(); int parameters_update();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll(); void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll(); void vehicle_manual_poll();
/**
* Check for set triplet updates.
*/
void vehicle_setpoint_poll(); void vehicle_setpoint_poll();
/**
* Check for global position updates.
*/
void global_pos_poll(); void global_pos_poll();
/**
* Check for vehicle status updates.
*/
void vehicle_status_poll(); void vehicle_status_poll();
/**
* Check for vehicle land detected updates.
*/
void vehicle_land_detected_poll(); void vehicle_land_detected_poll();
}; };