From da5d5a571276671a81ebbe6e504855e682ec1c24 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 6 Feb 2015 22:05:35 +0100 Subject: log velocity - and acceleration/thrust setpoint --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 38 +++++++++++++--------- 1 file changed, 22 insertions(+), 16 deletions(-) (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp') 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 b9692ffbf..886da8c20 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -982,22 +982,6 @@ MulticopterPositionControl::task_main() control_auto(dt); } - /* fill local position setpoint */ - _local_pos_sp.timestamp = hrt_absolute_time(); - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); - _local_pos_sp.yaw = _att_sp.yaw_body; - - /* publish local position setpoint */ - if (_local_pos_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - - } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); - } - - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); @@ -1298,6 +1282,11 @@ MulticopterPositionControl::task_main() _att_sp.thrust = thrust_abs; + /* save thrust setpoint for logging */ + _local_pos_sp.acc_x = thrust_sp(0); + _local_pos_sp.acc_x = thrust_sp(1); + _local_pos_sp.acc_x = thrust_sp(2); + _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ @@ -1313,6 +1302,23 @@ MulticopterPositionControl::task_main() } } + /* fill local position, velocity and thrust setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + _local_pos_sp.yaw = _att_sp.yaw_body; + _local_pos_sp.vx = _vel_sp(0); + _local_pos_sp.vy = _vel_sp(1); + _local_pos_sp.vz = _vel_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; -- cgit v1.2.3