diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 13:23:33 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 13:23:33 +0200 |
commit | f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476 (patch) | |
tree | 100ac05749e5c899d264de1d129d6d98935b2491 /src/modules/fixedwing_backside | |
parent | 3f9f1d60e03f501209deb6c7532c37dfb786f343 (diff) | |
parent | a013fc5d0b28cd72f41a28c8229c2d7ab99965f4 (diff) | |
download | px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.tar.gz px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.tar.bz2 px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.zip |
Merge branch 'seatbelt_multirotor' into seatbelt_multirotor_new
WIP, TODO fixedwing
Diffstat (limited to 'src/modules/fixedwing_backside')
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing.cpp | 171 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 6 |
2 files changed, 80 insertions, 97 deletions
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 16fcbd864..d65045d68 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -39,8 +39,6 @@ #include "fixedwing.hpp" -#if 0 - namespace control { @@ -157,9 +155,8 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; -#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -168,10 +165,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position -#warning should be base on flags // handle autopilot modes if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || - _status.navigation_state == NAVIGATION_STATE_SEATBELT) { + _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); @@ -223,93 +219,83 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ -#warning fix this - // 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.navigation_state == NAVIGATION_STATE_MANUAL) { - -#warning fix here too -// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { -// _actuators.control[CH_AIL] = _manual.roll; -// _actuators.control[CH_ELV] = _manual.pitch; -// _actuators.control[CH_RDR] = _manual.yaw; -// _actuators.control[CH_THR] = _manual.throttle; -// -// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - // 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.vx * _pos.vx + - _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); - - // pitch channel -> rate of climb - // TODO, might want to put a gain on this, otherwise commanding - // from +1 -> -1 m/s for rate of climb - //float dThrottle = _cr2Thr.update( - //_crMax.get()*_manual.pitch - _pos.vz); - - // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); - - // throttle channel -> velocity - // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - - // currently using manual throttle - // XXX if you enable this watch out, vz might be very noisy - //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; - - // 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 */ -#warning fix this - // 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; - // } -#warning fix this -// } - - // body rates controller, disabled for now - else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + 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; + } + + } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + // 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.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // pitch channel -> rate of climb + // TODO, might want to put a gain on this, otherwise commanding + // from +1 -> -1 m/s for rate of climb + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); + + // roll channel -> bank angle + float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.roll); + + // throttle channel -> velocity + // negative sign because nose over to increase speed + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + + // yaw rate cmd + float rCmd = 0; + + // stabilization + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + + // currently using manual throttle + // XXX if you enable this watch out, vz might be very noisy + //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.control[CH_THR] = _manual.throttle; + + // 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.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; } + // 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); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications @@ -330,4 +316,3 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() } // namespace control -#endif diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index b0de69f55..f4ea05088 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -151,14 +151,12 @@ int control_demo_thread_main(int argc, char *argv[]) using namespace control; -#warning fix this -// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); + fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); thread_running = true; while (!thread_should_exit) { -#warning fix this -// autopilot.update(); + autopilot.update(); } warnx("exiting."); |