fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 11:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed wing backside controller: Fix code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-05 22:16:12 +02:00
parent b678a554ea
commit 543f1f0ce3
2 changed files with 39 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fixedwing_backside/fixedwing.cpp
+32 -27
View File
@@ -58,8 +58,8 @@ BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
_rudder = outputScale*_r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
_rudder = outputScale * _r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
@@ -79,9 +79,9 @@ BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r, float outputScale)
{
_aileron = outputScale*_p2Ail.update(
_aileron = outputScale * _p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = outputScale*_q2Elv.update(
_elevator = outputScale * _q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r, outputScale);
}
@@ -127,7 +127,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
if (poll(&_attPoll, 1, 100) < 0) { return; } // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
@@ -135,7 +135,7 @@ void BlockMultiModeBacksideAutopilot::update()
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
if (dt > 1.0f || dt < 0) { return; }
// set dt for all child blocks
setDt(dt);
@@ -146,14 +146,15 @@ void BlockMultiModeBacksideAutopilot::update()
}
// check for new updates
if (_param_update.updated()) updateParams();
if (_param_update.updated()) { updateParams(); }
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
}
// only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) {
@@ -170,13 +171,13 @@ void BlockMultiModeBacksideAutopilot::update()
if (_status.main_state == MAIN_STATE_AUTO) {
// calculate velocity, XXX should be airspeed,
// but using ground speed for now for the purpose
// but using ground speed for now for the purpose
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
@@ -198,8 +199,8 @@ void BlockMultiModeBacksideAutopilot::update()
float rCmd = 0;
// stabilization
float velocityRatio = _trimV.get()/v;
float outputScale = velocityRatio*velocityRatio;
float velocityRatio = _trimV.get() / v;
float outputScale = velocityRatio * velocityRatio;
// this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
@@ -230,15 +231,15 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
@@ -253,8 +254,8 @@ void BlockMultiModeBacksideAutopilot::update()
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
@@ -263,7 +264,7 @@ void BlockMultiModeBacksideAutopilot::update()
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@@ -284,14 +285,17 @@ void BlockMultiModeBacksideAutopilot::update()
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
_actuators.control[CH_THR] : _manual.throttle;
}
// body rates controller, disabled for now
// TODO
} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
// body rates controller, disabled for now
// TODO
} else if (
0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
_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_ELV] = _stabilization.getElevator();
@@ -307,8 +311,9 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
}
updatePublications();
}
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+7 -6
View File
@@ -79,8 +79,9 @@ static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n");
exit(1);
@@ -112,11 +113,11 @@ int fixedwing_backside_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("fixedwing_backside",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
control_demo_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
control_demo_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}