diff options
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 |
1 files changed, 16 insertions, 15 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3b631e2ce..bf65d2805 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin <anton.babushkin@me.com> */ -#include <nuttx/config.h> -#include <stdio.h> +#include <px4.h> +#include <functional> +#include <cstdio> #include <stdlib.h> #include <string.h> #include <unistd.h> @@ -54,8 +55,7 @@ #include <poll.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> + #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> @@ -67,12 +67,13 @@ #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> +// #include <systemlib/param/param.h> +// #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <lib/geo/geo.h> #include <mavlink/mavlink_log.h> +#include <platforms/px4_defines.h> #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -531,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +988,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1148,11 +1149,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; + if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att.R, 2, 2); - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + } else if (PX4_R(_att.R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { @@ -1260,7 +1261,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1276,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; |