From da5d5a571276671a81ebbe6e504855e682ec1c24 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 6 Feb 2015 22:05:35 +0100 Subject: [PATCH] log velocity - and acceleration/thrust setpoint --- msg/vehicle_local_position_setpoint.msg | 8 +++- .../mc_pos_control/mc_pos_control_main.cpp | 38 +++++++++++-------- src/modules/sdlog2/sdlog2.c | 6 +++ src/modules/sdlog2/sdlog2_messages.h | 8 +++- 4 files changed, 42 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg index a2ff8a4ae7..927e9ab5ad 100644 --- a/msg/vehicle_local_position_setpoint.msg +++ b/msg/vehicle_local_position_setpoint.msg @@ -1,7 +1,13 @@ -# Local position in NED frame +# Local position setpoint in NED frame uint64 timestamp # timestamp of the setpoint float32 x # in meters NED float32 y # in meters NED float32 z # in meters NED float32 yaw # in radians NED -PI..+PI +float32 vx # in meters/sec +float32 vy # in meters/sec +float32 vz # in meters/sec +float32 acc_x # in meters/(sec*sec) +float32 acc_y # in meters/(sec*sec) +float32 acc_z # in meters/(sec*sec) 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 b9692ffbfc..886da8c209 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; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8ac87b238a..255432de4d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1536,6 +1536,12 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPSP.y = buf.local_pos_sp.y; log_msg.body.log_LPSP.z = buf.local_pos_sp.z; log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; + log_msg.body.log_LPSP.vx = buf.local_pos_sp.vx; + log_msg.body.log_LPSP.vy = buf.local_pos_sp.vy; + log_msg.body.log_LPSP.vz = buf.local_pos_sp.vz; + log_msg.body.log_LPSP.acc_x = buf.local_pos_sp.acc_x; + log_msg.body.log_LPSP.acc_y = buf.local_pos_sp.acc_y; + log_msg.body.log_LPSP.acc_z = buf.local_pos_sp.acc_z; LOGBUFFER_WRITE_AND_COUNT(LPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7080d9c316..06e639bf55 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -128,6 +128,12 @@ struct log_LPSP_s { float y; float z; float yaw; + float vx; + float vy; + float vz; + float acc_x; + float acc_y; + float acc_z; }; /* --- GPS - GPS POSITION --- */ @@ -471,7 +477,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), - LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), + LOG_FORMAT(LPSP, "ffffffffff", "X,Y,Z,Yaw,VX,VY,VZ,AX,AY,AZ"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),