diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-02-09 22:35:36 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-02-09 22:35:36 +0100 |
commit | 9e2e6ceac142b5c2c4c60c0841c3e554bf968680 (patch) | |
tree | d20009744e5f29d32bec9aec91aff3ae2ff5606c /src/modules/mc_pos_control | |
parent | 295ad3f28f6f7f795d49a78d527c02dcfdb9c1ab (diff) | |
parent | da5d5a571276671a81ebbe6e504855e682ec1c24 (diff) | |
download | px4-firmware-9e2e6ceac142b5c2c4c60c0841c3e554bf968680.tar.gz px4-firmware-9e2e6ceac142b5c2c4c60c0841c3e554bf968680.tar.bz2 px4-firmware-9e2e6ceac142b5c2c4c60c0841c3e554bf968680.zip |
Merge pull request #1752 from PX4/log_thrust_sp
log velocity - and acceleration/thrust setpoint
Diffstat (limited to 'src/modules/mc_pos_control')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 38 |
1 files changed, 22 insertions, 16 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 aafc56c87..0d9cc8157 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1011,22 +1011,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(); @@ -1327,6 +1311,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(); @@ -1335,6 +1324,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; |