aboutsummaryrefslogtreecommitdiff
path: root/src/examples
diff options
context:
space:
mode:
authorDaniel Agar <daniel@agar.ca>2015-03-03 11:47:27 -0500
committerLorenz Meier <lm@inf.ethz.ch>2015-03-19 23:49:36 +0100
commit662ec3f62f6c271d7746204888c2d55e3c3bd86e (patch)
tree55b9134a2fdc223e2261905d750104e6178c5df2 /src/examples
parent0eaf41a048074d5ebf6b84094cfd69ef10451180 (diff)
downloadpx4-firmware-662ec3f62f6c271d7746204888c2d55e3c3bd86e.tar.gz
px4-firmware-662ec3f62f6c271d7746204888c2d55e3c3bd86e.tar.bz2
px4-firmware-662ec3f62f6c271d7746204888c2d55e3c3bd86e.zip
examples fix code style
Diffstat (limited to 'src/examples')
-rw-r--r--src/examples/fixedwing_control/main.c43
-rw-r--r--src/examples/flow_position_estimator/flow_position_estimator_main.c181
-rw-r--r--src/examples/matlab_csv_serial/matlab_csv_serial.c57
-rw-r--r--src/examples/publisher/publisher_example.h9
-rw-r--r--src/examples/publisher/publisher_start_nuttx.cpp10
-rw-r--r--src/examples/px4_daemon_app/px4_daemon_app.c26
-rw-r--r--src/examples/px4_simple_app/px4_simple_app.c16
-rw-r--r--src/examples/rover_steering_control/main.cpp12
-rw-r--r--src/examples/subscriber/subscriber_example.cpp20
-rw-r--r--src/examples/subscriber/subscriber_example.h5
-rw-r--r--src/examples/subscriber/subscriber_start_nuttx.cpp10
11 files changed, 201 insertions, 188 deletions
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index d8b777b91..17b27290c 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -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);
}
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index da4ea825b..f8e3f2dc5 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -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");
}
diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
index 145cf99cc..8e439bdac 100644
--- a/src/examples/matlab_csv_serial/matlab_csv_serial.c
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -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);
}
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 8165b0ffc..0a66d3edc 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -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;
};
diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp
index db9d85269..405b3c987 100644
--- a/src/examples/publisher/publisher_start_nuttx.cpp
+++ b/src/examples/publisher/publisher_start_nuttx.cpp
@@ -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);
}
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index 45d541137..860b1af78 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -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");
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 4e9f099ed..e22c59fa2 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -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) {}
*/
diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp
index 7ef914098..0f8eb800e 100644
--- a/src/examples/rover_steering_control/main.cpp
+++ b/src/examples/rover_steering_control/main.cpp
@@ -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);
}
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 496e1dcd8..5a23dc80b 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -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();
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index 686fed023..9eb5dd2a0 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -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);
diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp
index 6129b19ac..b450230c1 100644
--- a/src/examples/subscriber/subscriber_start_nuttx.cpp
+++ b/src/examples/subscriber/subscriber_start_nuttx.cpp
@@ -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);
}