fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 01:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mocap_support: added support for mocap data on firmware

Browse Source
This commit is contained in:
TSC21
2015-06-13 17:03:31 +01:00
parent c92655b850
commit dccd4df7bc
13 changed files with 225 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+16 -15
View File
@@ -80,7 +80,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -1071,7 +1071,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct att_pos_mocap_s att_pos_mocap;
struct vision_position_estimate_s vision_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
@@ -1127,7 +1127,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST0_s log_EST0;
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_MOCP_s log_MOCP;
struct log_VISN_s log_VISN;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
@@ -1162,7 +1162,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int triplet_sub;
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int att_pos_mocap_sub;
int vision_pos_sub;
int flow_sub;
int rc_sub;
@@ -1197,7 +1197,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.local_pos_sp_sub = -1;
subs.global_pos_sub = -1;
subs.triplet_sub = -1;
subs.vicon_pos_sub = -1;
subs.att_pos_mocap_sub = -1;
subs.vision_pos_sub = -1;
subs.flow_sub = -1;
subs.rc_sub = -1;
@@ -1681,16 +1681,17 @@ int sdlog2_thread_main(int argc, char *argv[])
}
}
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
log_msg.msg_type = LOG_VICN_MSG;
log_msg.body.log_VICN.x = buf.vicon_pos.x;
log_msg.body.log_VICN.y = buf.vicon_pos.y;
log_msg.body.log_VICN.z = buf.vicon_pos.z;
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
/* --- MOCAP ATTITUDE AND POSITION --- */
if (copy_if_updated(ORB_ID(att_pos_mocap), &subs.att_pos_mocap_sub, &buf.att_pos_mocap)) {
log_msg.msg_type = LOG_MOCP_MSG;
log_msg.body.log_MOCP.qw = buf.att_pos_mocap.q[0];
log_msg.body.log_MOCP.qx = buf.att_pos_mocap.q[1];
log_msg.body.log_MOCP.qy = buf.att_pos_mocap.q[2];
log_msg.body.log_MOCP.qz = buf.att_pos_mocap.q[3];
log_msg.body.log_MOCP.x = buf.att_pos_mocap.x;
log_msg.body.log_MOCP.y = buf.att_pos_mocap.y;
log_msg.body.log_MOCP.z = buf.att_pos_mocap.z;
LOGBUFFER_WRITE_AND_COUNT(MOCP);
}
/* --- VISION POSITION --- */
src/modules/sdlog2/sdlog2_messages.h
+10 -9
View File
@@ -321,15 +321,16 @@ struct log_PWR_s {
uint8_t high_power_rail_overcurrent;
};
/* --- VICN - VICON POSITION --- */
#define LOG_VICN_MSG 25
struct log_VICN_s {
/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
#define LOG_MOCP_MSG 25
struct log_MOCP_s {
float qw;
float qx;
float qy;
float qz;
float x;
float y;
float z;
float roll;
float pitch;
float yaw;
};
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
@@ -427,10 +428,10 @@ struct log_VISN_s {
float vx;
float vy;
float vz;
float qw;
float qx;
float qy;
float qz;
float qw;
};
/* --- ENCODERS - ENCODER DATA --- */
@@ -523,8 +524,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),