diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index bbb39f20f0b195935ea96007af253676efd43050..7984bb12afceac03a18e384b34432ea4c6230b50 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -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();
}
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index 06559de9790bff9f2c19bd0914f3c5fe95da6c70..fe9a1066bd4f06c0341f7464e46d2513241d8eed 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -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);
}