dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1752 from PX4/log_thrust_sp 

log velocity - and acceleration/thrust setpoint
This commit is contained in:
Lorenz Meier 2015-02-09 22:35:36 +01:00
parent 295ad3f28f da5d5a5712
commit 9e2e6ceac1
4 changed files with 42 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_local_position_setpoint.msg
View File

@ -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)

38
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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;

6
src/modules/sdlog2/sdlog2.c
View File

@ -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);
}

8
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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"),