fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 08:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'seatbelt_multirotor' into seatbelt_multirotor_new

Browse Source 
WIP, TODO fixedwing
This commit is contained in:
Anton Babushkin
2013-08-16 13:23:33 +02:00
parent 3f9f1d60e0 a013fc5d0b
commit f78666e0c8
31 changed files with 1931 additions and 807 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fixedwing_backside/fixedwing.cpp
+78 -93
View File
@@ -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
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+2 -4
View File
@@ -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.");