fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 14:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Clean 250 Hz updates in filter, partial updates enabled

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-03 13:39:26 +02:00
parent 992a415ffc
commit affa3af4e6
4 changed files with 82 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/sdlog/sdlog.c
+36 -1
View File
@@ -60,6 +60,9 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <systemlib/systemlib.h>
@@ -273,6 +276,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct vehicle_command_s cmd;
struct vehicle_local_position_s local_pos;
struct vehicle_global_position_s global_pos;
struct vehicle_gps_position_s gps_pos;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -283,6 +289,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
int spa_sub;
int act_0_sub;
int controls0_sub;
int local_pos_sub;
int global_pos_sub;
int gps_pos_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -329,6 +338,27 @@ int sdlog_thread_main(int argc, char *argv[]) {
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
/* subscribe to ORB for local position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION --- */
/* subscribe to ORB for global position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
fds[fdsc_count].fd = subs.global_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS POSITION --- */
/* subscribe to ORB for global position */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
fds[fdsc_count].fd = subs.gps_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -435,6 +465,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
#pragma pack(push, 1)
struct {
@@ -450,6 +483,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
float actuators[8];
float vbat;
float adc[3];
float local_pos[3];
} sysvector = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -462,7 +496,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
.vbat = buf.raw.battery_voltage_v,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}
};
#pragma pack(pop)