zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Update to fixedwing backside.

sbg
jgoppert 9 years ago
parent
b9e461b028
commit
f0ad724fda
1 changed files with 59 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 110
      src/modules/fixedwing_backside/fixedwing.cpp

110
src/modules/fixedwing_backside/fixedwing.cpp
Unescape View File

@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update()
// store old position command before update if new command sent // store old position command before update if new command sent
if (_missionCmd.updated()) { if (_missionCmd.updated()) {
_lastMissionCmd = _missionCmd.getData(); _lastMissionCmd = _missionCmd.get();
} }
// check for new updates // check for new updates
@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update()
updateSubscriptions(); updateSubscriptions();
// default all output to zero unless handled by mode // default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) { for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f; _actuators.get().control[i] = 0.0f;
} }
// only update guidance in auto mode // only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) { if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
// TODO use vehicle_control_mode here? // TODO use vehicle_control_mode here?
// update guidance // update guidance
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); _guide.update(_pos.get(), _att.get(),
_missionCmd.get().current,
_lastMissionCmd.current);
} }
// XXX handle STABILIZED (loiter on spot) as well // XXX handle STABILIZED (loiter on spot) as well
@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position // the setpoint should update to loitering around this position
// handle autopilot modes // handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO) { if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) {
// calculate velocity, XXX should be airspeed, // 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 // of control we will limit the velocity feedback between
// the min/max velocity // the min/max velocity
float v = _vLimit.update(sqrtf( float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n + _pos.get().vel_n * _pos.get().vel_n +
_pos.vel_e * _pos.vel_e + _pos.get().vel_e * _pos.get().vel_e +
_pos.vel_d * _pos.vel_d)); _pos.get().vel_d * _pos.get().vel_d));
// limit velocity command between min/max velocity // limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get()); float vCmd = _vLimit.update(_vCmd.get());
// altitude hold // altitude hold
float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt);
// heading hold // heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
float pCmd = _phi2P.update(phiCmd - _att.roll); float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// velocity hold // velocity hold
// negative sign because nose over to increase speed // negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch); float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd // yaw rate cmd
float rCmd = 0; float rCmd = 0;
@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update()
float outputScale = velocityRatio * velocityRatio; float outputScale = velocityRatio * velocityRatio;
// this term scales the output based on the dynamic pressure change from trim // this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd, _stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed, _att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed,
outputScale); outputScale);
// output // output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.control[CH_THR] = dThrottle + _trimThr.get(); _actuators.get().control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now. // XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once // If it turns out to be confusing, it can be removed later once
@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice. // This is not a hack, but a design choice.
// do not limit in HIL // do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) { if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) {
/* limit to value of manual throttle */ /* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.get().control[CH_THR] =
_actuators.control[CH_THR] : _manual.throttle; (_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
} }
} else if (_status.main_state == MAIN_STATE_MANUAL) { } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll; _actuators.get().control[CH_AIL] = _manual.get().y;
_actuators.control[CH_ELV] = _manual.pitch; _actuators.get().control[CH_ELV] = _manual.get().x;
_actuators.control[CH_RDR] = _manual.yaw; _actuators.get().control[CH_RDR] = _manual.get().r;
_actuators.control[CH_THR] = _manual.throttle; _actuators.get().control[CH_THR] = _manual.get().z;
} else if (_status.main_state == MAIN_STATE_ALTCTL || } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now // calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between // for the purpose of control we will limit the velocity feedback between
// the min/max velocity // the min/max velocity
float v = _vLimit.update(sqrtf( float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n + _pos.get().vel_n * _pos.get().vel_n +
_pos.vel_e * _pos.vel_e + _pos.get().vel_e * _pos.get().vel_e +
_pos.vel_d * _pos.vel_d)); _pos.get().vel_d * _pos.get().vel_d));
// pitch channel -> rate of climb // pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding // TODO, might want to put a gain on this, otherwise commanding
@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update()
//_crMax.get()*_manual.pitch - _pos.vz); //_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle // roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll); float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// throttle channel -> velocity // throttle channel -> velocity
// negative sign because nose over to increase speed // negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle * float vCmd = _vLimit.update(_manual.get().z *
(_vLimit.getMax() - _vLimit.getMin()) + (_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin()); _vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch); float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd // yaw rate cmd
float rCmd = 0; float rCmd = 0;
// stabilization // stabilization
_stabilization.update(pCmd, qCmd, rCmd, _stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed); _att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed);
// output // output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currently using manual throttle // currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy // XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get(); //_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle; _actuators.get().control[CH_THR] = _manual.get().z;
// XXX limit throttle to manual setting (safety) for now. // XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once // If it turns out to be confusing, it can be removed later once
@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice. // This is not a hack, but a design choice.
/* do not limit in HIL */ /* do not limit in HIL */
if (_status.hil_state != HIL_STATE_ON) { if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) {
/* limit to value of manual throttle */ /* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.get().control[CH_THR] =
_actuators.control[CH_THR] : _manual.throttle; (_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
} }
// body rates controller, disabled for now // body rates controller, disabled for now
@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update()
} else if ( } else if (
0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, _stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r,
_att.rollspeed, _att.pitchspeed, _att.yawspeed); _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron(); _actuators.get().control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator(); _actuators.get().control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder(); _actuators.get().control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle; _actuators.get().control[CH_THR] = _manual.get().z;
} }
// update all publications // update all publications
@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{ {
// send one last publication when destroyed, setting // send one last publication when destroyed, setting
// all output to zero // all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f; _actuators.get().control[i] = 0.0f;
} }
updatePublications(); updatePublications();

Write Preview
Loading…
Cancel
Save