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

move vehicle_attitude_setpoint to msg format

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-08 10:37:01 +01:00
parent 65629d09d5
commit 87df7c3243
11 changed files with 66 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+11 -11
View File
@@ -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,8 +67,8 @@
#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>
@@ -532,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 +987,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;
@@ -1260,7 +1260,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 +1275,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;