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

invert setpoint formula

Browse Source
This commit is contained in:
sander
2016-04-11 14:56:49 +02:00
committed by tumbili
parent e11d4ae302
commit cf309bfc3f
1 changed files with 7 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+2 -21
View File
@@ -284,12 +284,6 @@ void Standard::update_mc_state()
{ {
VtolType::update_mc_state(); VtolType::update_mc_state();
// get desired rotation matrix
math::Matrix<3,3> R_sp(&_v_att_sp->R_body[0]);
// get rotation matrix
math::Matrix<3,3> R(&_v_att->R[0]);
// get projection of thrust vector on body x axis. This is used to // get projection of thrust vector on body x axis. This is used to
// determine the desired forward acceleration which we want to achieve with the pusher // determine the desired forward acceleration which we want to achieve with the pusher
math::Matrix<3,3> R(&_v_att->R[0]); math::Matrix<3,3> R(&_v_att->R[0]);
@@ -342,23 +336,8 @@ void Standard::fill_actuator_outputs()
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight; // roll * _mc_roll_weight; // roll
if(_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] < -0.05f){
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = -0.02f;
_pusher_throttle = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * -0.15f;
if(!_pusher_active){
_pusher_active = true;
warnx("Activating pusher");
}
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch
if(_pusher_active){
_pusher_active = false;
warnx("Deactivating pusher");
}
}
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
@@ -375,6 +354,8 @@ void Standard::fill_actuator_outputs()
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
* (1 - _mc_yaw_weight); // yaw * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
// set the fixed wing throttle control // set the fixed wing throttle control
if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) {
// take the throttle value commanded by the fw controller // take the throttle value commanded by the fw controller