diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-30 11:01:09 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-30 11:01:09 +0100 |
commit | ef065808a33d482ebea2715c4b4f0b5a936f1a73 (patch) | |
tree | b17d739ab31f72f00611e18b8cbd055f3ff65a41 /src/modules/fw_att_control/fw_att_control_main.cpp | |
parent | d8eefa30538331fde7e5ce79fef4e04dc62664bc (diff) | |
parent | 4942883ddcb5d1a09e96335b1edbbf2d937937b4 (diff) | |
download | px4-firmware-ef065808a33d482ebea2715c4b4f0b5a936f1a73.tar.gz px4-firmware-ef065808a33d482ebea2715c4b4f0b5a936f1a73.tar.bz2 px4-firmware-ef065808a33d482ebea2715c4b4f0b5a936f1a73.zip |
Merged master
Diffstat (limited to 'src/modules/fw_att_control/fw_att_control_main.cpp')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 40 |
1 files changed, 25 insertions, 15 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 598408c9f..260b25a48 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -335,8 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_0_pub(-1), _actuators_2_pub(-1), - _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), - _actuators_id(ORB_ID(actuator_controls_0)), + _rates_sp_id(0), + _actuators_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -588,12 +588,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_fw); - } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); + if (!_rates_sp_id) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } } @@ -719,22 +721,24 @@ FixedwingAttitudeControl::task_main() R.set(_att.R); R_adapted.set(_att.R); - //move z to x + /* move z to x */ R_adapted(0, 0) = R(0, 2); R_adapted(1, 0) = R(1, 2); R_adapted(2, 0) = R(2, 2); - //move x to z + + /* move x to z */ R_adapted(0, 2) = R(0, 0); R_adapted(1, 2) = R(1, 0); R_adapted(2, 2) = R(2, 0); - //change direction of pitch (convert to right handed system) + /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); - //fill in new attitude data + + /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); @@ -748,7 +752,7 @@ FixedwingAttitudeControl::task_main() _att.R[2][1] = R_adapted(2, 1); _att.R[2][2] = R_adapted(2, 2); - // lastly, roll- and yawspeed have to be swaped + /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; @@ -819,6 +823,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if @@ -879,6 +884,8 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + /* allow manual control of rudder deflection */ + yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -978,6 +985,9 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + + /* add in manual rudder control */ + _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1017,7 +1027,7 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); - } else { + } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } @@ -1042,7 +1052,7 @@ FixedwingAttitudeControl::task_main() /* publish the actuator controls */ if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else { + } else if (_actuators_id) { _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } |