diff options
Diffstat (limited to 'src/examples')
-rw-r--r-- | src/examples/fixedwing_control/main.c | 43 | ||||
-rw-r--r-- | src/examples/flow_position_estimator/flow_position_estimator_main.c | 181 | ||||
-rw-r--r-- | src/examples/matlab_csv_serial/matlab_csv_serial.c | 57 | ||||
-rw-r--r-- | src/examples/publisher/publisher_example.h | 9 | ||||
-rw-r--r-- | src/examples/publisher/publisher_start_nuttx.cpp | 10 | ||||
-rw-r--r-- | src/examples/px4_daemon_app/px4_daemon_app.c | 26 | ||||
-rw-r--r-- | src/examples/px4_simple_app/px4_simple_app.c | 16 | ||||
-rw-r--r-- | src/examples/rover_steering_control/main.cpp | 12 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.cpp | 20 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.h | 5 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_start_nuttx.cpp | 10 |
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); } |