examples fix code style

This commit is contained in:
Daniel Agar
2015-03-03 11:47:27 -05:00
committed by Lorenz Meier
parent 0eaf41a048
commit 662ec3f62f
11 changed files with 201 additions and 188 deletions

View File

@@ -112,8 +112,9 @@ static void usage(const char *reason);
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
* Control heading.
@@ -128,7 +129,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
* @param att_sp The attitude setpoint. This is the output of the controller
*/
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
/* Variables */
static bool thread_should_exit = false; /**< Daemon exit flag */
@@ -137,8 +138,9 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
/*
@@ -175,7 +177,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
}
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
{
/*
@@ -192,6 +194,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p
/* limit output, this commonly is a tuning parameter, too */
if (att_sp->roll_body < -0.6f) {
att_sp->roll_body = -0.6f;
} else if (att_sp->roll_body > 0.6f) {
att_sp->roll_body = 0.6f;
}
@@ -285,7 +288,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* Setup of loop */
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
{ .fd = att_sub, .events = POLLIN }
};
while (!thread_should_exit) {
@@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (manual_sp_updated)
/* get the RC (or otherwise user based) input */
{
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
}
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (isfinite(manual_sp.z) &&
(manual_sp.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) {
(manual_sp.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) {
}
/* get the system status and the flight mode we're in */
@@ -378,8 +384,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
@@ -393,8 +400,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
static void
usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
exit(1);
@@ -410,8 +418,9 @@ usage(const char *reason)
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
@@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = task_spawn_cmd("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
thread_running = true;
exit(0);
}

View File

@@ -81,8 +81,10 @@ static void usage(const char *reason);
static void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@@ -97,13 +99,12 @@ static void usage(const char *reason)
*/
int flow_position_estimator_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("flow position estimator already running\n");
/* this is not an error */
exit(0);
@@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4000,
flow_position_estimator_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4000,
flow_position_estimator_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop"))
{
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status"))
{
if (thread_running)
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tflow position estimator is running\n");
else
} else {
printf("\tflow position estimator not started\n");
}
exit(0);
}
@@ -147,9 +148,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 },
{ 1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated
const float time_scale = powf(10.0f,-6.0f);
{ 1, 0, 0 },
{ 0, 0, 1 }
}; // 90deg rotated
const float time_scale = powf(10.0f, -6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f};
static float flow_speed[3] = {0.0f, 0.0f, 0.0f};
static float global_speed[3] = {0.0f, 0.0f, 0.0f};
@@ -192,18 +194,18 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* init local position and filtered flow struct */
struct vehicle_local_position_s local_pos = {
.x = 0.0f,
.y = 0.0f,
.z = 0.0f,
.vx = 0.0f,
.vy = 0.0f,
.vz = 0.0f
.x = 0.0f,
.y = 0.0f,
.z = 0.0f,
.vx = 0.0f,
.vy = 0.0f,
.vz = 0.0f
};
struct filtered_bottom_flow_s filtered_flow = {
.sumx = 0.0f,
.sumy = 0.0f,
.vx = 0.0f,
.vy = 0.0f
.sumx = 0.0f,
.sumy = 0.0f,
.vx = 0.0f,
.vy = 0.0f
};
/* advert pub messages */
@@ -224,37 +226,29 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");
while (!thread_should_exit)
{
while (!thread_should_exit) {
if (sensors_ready)
{
if (sensors_ready) {
/*This runs at the rate of the sensors */
struct pollfd fds[2] = {
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN }
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0)
{
if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
} else if (ret == 0) {
/* no return value, ignore */
// printf("[flow position estimator] no bottom flow.\n");
}
else
{
} else {
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
@@ -264,8 +258,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
/* only if flow data changed */
if (fds[0].revents & POLLIN)
{
if (fds[0].revents & POLLIN) {
perf_begin(mc_loop_perf);
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
@@ -284,46 +277,48 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
* -> minimum sonar value 0.3m
*/
if (!vehicle_liftoff)
{
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
if (!vehicle_liftoff) {
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) {
vehicle_liftoff = true;
}
else
{
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
}
} else {
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) {
vehicle_liftoff = false;
}
}
/* calc dt between flow timestamps */
/* ignore first flow msg */
if(time_last_flow == 0)
{
if (time_last_flow == 0) {
time_last_flow = flow.timestamp;
continue;
}
dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
time_last_flow = flow.timestamp;
/* only make position update if vehicle is lift off or DEBUG is activated*/
if (vehicle_liftoff || params.debug)
{
if (vehicle_liftoff || params.debug) {
/* copy flow */
if (flow.integration_timespan > 0) {
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
} else {
flow_speed[0] = 0;
flow_speed[1] = 0;
}
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */
for(uint8_t i = 0; i < 3; i++)
{
for (uint8_t i = 0; i < 3; i++) {
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
for (uint8_t j = 0; j < 3; j++) {
sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];
}
speed[i] = sum;
}
@@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* convert to globalframe velocity
* -> local position is currently not used for position control
*/
for(uint8_t i = 0; i < 3; i++)
{
for (uint8_t i = 0; i < 3; i++) {
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
for (uint8_t j = 0; j < 3; j++) {
sum = sum + speed[j] * PX4_R(att.R, i, j);
}
global_speed[i] = sum;
}
@@ -354,9 +350,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
local_pos.vy = global_speed[1];
local_pos.xy_valid = true;
local_pos.v_xy_valid = true;
}
else
{
} else {
/* set speed to zero and let position as it is */
filtered_flow.vx = 0;
filtered_flow.vy = 0;
@@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
/* filtering ground distance */
if (!vehicle_liftoff || !armed.armed)
{
if (!vehicle_liftoff || !armed.armed) {
/* not possible to fly */
sonar_valid = false;
local_pos.z = 0.0f;
local_pos.z_valid = false;
}
else
{
} else {
sonar_valid = true;
}
if (sonar_valid || params.debug)
{
if (sonar_valid || params.debug) {
/* simple lowpass sonar filtering */
/* if new value or with sonar update frequency */
if (sonar_new != sonar_last || counter % 10 == 0)
{
if (sonar_new != sonar_last || counter % 10 == 0) {
sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
sonar_last = sonar_new;
}
@@ -392,12 +383,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
float height_diff = sonar_new - sonar_lp;
/* if over 1/2m spike follow lowpass */
if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold)
{
if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) {
local_pos.z = -sonar_lp;
}
else
{
} else {
local_pos.z = -sonar_new;
}
@@ -408,15 +397,14 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
local_pos.timestamp = hrt_absolute_time();
/* publish local position */
if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
&& isfinite(local_pos.vx) && isfinite(local_pos.vy))
{
if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
&& isfinite(local_pos.vx) && isfinite(local_pos.vy)) {
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}
/* publish filtered flow */
if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy))
{
if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx)
&& isfinite(filtered_flow.vy)) {
orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
}
@@ -427,9 +415,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
}
}
else
{
} else {
/* sensors not ready waiting for first attitude msg */
/* polling */
@@ -440,19 +426,16 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* wait for a attitude message, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
} else if (ret == 0) {
/* no return value, ignore */
printf("[flow position estimator] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN){
} else {
if (fds[0].revents & POLLIN) {
sensors_ready = true;
printf("[flow position estimator] initialized.\n");
}

View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -73,8 +73,10 @@ static void usage(const char *reason);
static void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@@ -89,13 +91,12 @@ static void usage(const char *reason)
*/
int matlab_csv_serial_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running\n");
/* this is not an error */
exit(0);
@@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop"))
{
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status"))
{
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("running");
} else {
warnx("stopped");
}
@@ -139,7 +139,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
errx(1, "need a serial port name as argument");
}
const char* uart_name = argv[1];
const char *uart_name = argv[1];
warnx("opening port %s", uart_name);
@@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
thread_running = true;
while (!thread_should_exit)
{
while (!thread_should_exit) {
/*This runs at the rate of the sensors */
struct pollfd fds[] = {
{ .fd = accel0_sub, .events = POLLIN }
{ .fd = accel0_sub, .events = POLLIN }
};
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
if (ret < 0)
{
if (ret < 0) {
/* poll error, ignore */
}
else if (ret == 0)
{
} else if (ret == 0) {
/* no return value, ignore */
warnx("no sensor data");
}
else
{
} else {
/* accel0 update available? */
if (fds[0].revents & POLLIN)
{
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
// write out on accel 0, but collect for all other sensors as they have updates
dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw,
(int)accel0.z_raw,
(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
}

View File

@@ -40,7 +40,8 @@
#pragma once
#include <px4.h>
class PublisherExample {
class PublisherExample
{
public:
PublisherExample();
@@ -49,7 +50,7 @@ public:
int main();
protected:
px4::NodeHandle _n;
px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub;
px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub;
px4::Publisher<px4::px4_rc_channels> *_rc_channels_pub;
px4::Publisher<px4::px4_vehicle_attitude> *_v_att_pub;
px4::Publisher<px4::px4_parameter_update> *_parameter_update_pub;
};

View File

@@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[])
task_should_exit = false;
daemon_task = task_spawn_cmd("publisher",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}

View File

@@ -34,7 +34,7 @@
/**
* @file px4_daemon_app.c
* daemon application example for PX4 autopilot
*
*
* @author Example User <mail@example.com>
*/
@@ -71,8 +71,10 @@ static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
if (reason) {
warnx("%s\n", reason);
}
errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
}
@@ -80,14 +82,15 @@ usage(const char *reason)
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
*
* The actual stack size should be set in the call
* to task_create().
*/
int px4_daemon_app_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
@@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
px4_daemon_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
px4_daemon_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}
@@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\trunning\n");
} else {
warnx("\tnot started\n");
}
exit(0);
}
@@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[])
exit(1);
}
int px4_daemon_thread_main(int argc, char *argv[]) {
int px4_daemon_thread_main(int argc, char *argv[])
{
warnx("[daemon] starting\n");

View File

@@ -75,30 +75,33 @@ int px4_simple_app_main(int argc, char *argv[])
for (int i = 0; i < 5; i++) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
printf("[px4_simple_app] Got no data within a second\n");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
printf("[px4_simple_app] ERROR return value from poll(): %d\n"
, poll_ret);
, poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
@@ -106,6 +109,7 @@ int px4_simple_app_main(int argc, char *argv[])
att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/

View File

@@ -267,18 +267,27 @@ int rover_steering_control_thread_main(int argc, char *argv[])
/* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
struct pollfd fds[2];
fds[0].fd = param_sub;
fds[0].events = POLLIN;
fds[1].fd = att_sub;
fds[1].events = POLLIN;
while (!thread_should_exit) {
@@ -371,6 +380,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) {
actuators.control[i] = 0.0f;
}
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
@@ -421,7 +431,7 @@ int rover_steering_control_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 20,
2048,
rover_steering_control_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
thread_running = true;
exit(0);
}

View File

@@ -43,7 +43,8 @@
using namespace px4;
void rc_channels_callback_function(const px4_rc_channels &msg) {
void rc_channels_callback_function(const px4_rc_channels &msg)
{
PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
}
@@ -81,21 +82,24 @@ SubscriberExample::SubscriberExample() :
* This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
* Also the current value of the _sub_rc_chan subscription is printed
*/
void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg)
{
PX4_INFO("rc_channels_callback (method): [%llu]",
msg.data().timestamp_last_valid);
msg.data().timestamp_last_valid);
PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
_sub_rc_chan->data().timestamp_last_valid);
_sub_rc_chan->data().timestamp_last_valid);
}
void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg)
{
PX4_INFO("vehicle_attitude_callback (method): [%llu]",
msg.data().timestamp);
msg.data().timestamp);
}
void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) {
void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg)
{
PX4_INFO("parameter_update_callback (method): [%llu]",
msg.data().timestamp);
msg.data().timestamp);
_p_sub_interv.update();
PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get());
_p_test_float.update();

View File

@@ -43,7 +43,8 @@ using namespace px4;
void rc_channels_callback_function(const px4_rc_channels &msg);
class SubscriberExample {
class SubscriberExample
{
public:
SubscriberExample();
@@ -54,7 +55,7 @@ protected:
px4::NodeHandle _n;
px4::ParameterInt _p_sub_interv;
px4::ParameterFloat _p_test_float;
px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
px4::Subscriber<px4_rc_channels> *_sub_rc_chan;
void rc_channels_callback(const px4_rc_channels &msg);

View File

@@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[])
task_should_exit = false;
daemon_task = task_spawn_cmd("subscriber",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0);
}