diff options
author | tumbili <bapstr@ethz.ch> | 2015-02-06 22:05:35 +0100 |
---|---|---|
committer | tumbili <bapstr@ethz.ch> | 2015-02-07 17:45:06 +0100 |
commit | da5d5a571276671a81ebbe6e504855e682ec1c24 (patch) | |
tree | 0b8a4707a03df01fd46cf4e11a171e5bc756c431 /src/modules | |
parent | b25808467c62adbc7ac1089e920263ee12fb65fd (diff) | |
download | px4-firmware-da5d5a571276671a81ebbe6e504855e682ec1c24.tar.gz px4-firmware-da5d5a571276671a81ebbe6e504855e682ec1c24.tar.bz2 px4-firmware-da5d5a571276671a81ebbe6e504855e682ec1c24.zip |
log velocity - and acceleration/thrust setpoint
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 38 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 6 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 8 |
3 files changed, 35 insertions, 17 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 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; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8ac87b238..255432de4 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 7080d9c31..06e639bf5 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"), |