Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

examples fix code style

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/fixedwing_control/main.c
+26 -17
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 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. * @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, void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct actuator_controls_s *actuators); struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/** /**
* Control heading. * 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 * @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, 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 */ /* Variables */
static bool thread_should_exit = false; /**< Daemon exit flag */ 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 params p;
static struct param_handles ph; 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, void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
struct actuator_controls_s *actuators) 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, 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 */ /* limit output, this commonly is a tuning parameter, too */
if (att_sp->roll_body < -0.6f) { if (att_sp->roll_body < -0.6f) {
att_sp->roll_body = -0.6f; att_sp->roll_body = -0.6f;
} else if (att_sp->roll_body > 0.6f) { } else if (att_sp->roll_body > 0.6f) {
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 */ /* Setup of loop */
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }}; { .fd = att_sub, .events = POLLIN }
};
while (!thread_should_exit) { while (!thread_should_exit) {
@@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (manual_sp_updated) if (manual_sp_updated)
/* get the RC (or otherwise user based) input */ /* get the RC (or otherwise user based) input */
{
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); 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 */ /* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (isfinite(manual_sp.z) && if (isfinite(manual_sp.z) &&
(manual_sp.z >= 0.6f) && (manual_sp.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) { (manual_sp.z <= 1.0f)) {
} }
/* get the system status and the flight mode we're in */ /* 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; thread_running = false;
/* kill all outputs */ /* 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; actuators.control[i] = 0.0f;
}
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); 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 static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) if (reason) {
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
exit(1); exit(1);
@@ -410,8 +418,9 @@ usage(const char *reason)
*/ */
int ex_fixedwing_control_main(int argc, char *argv[]) int ex_fixedwing_control_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
usage("missing command"); usage("missing command");
}
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
@@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
deamon_task = task_spawn_cmd("ex_fixedwing_control", deamon_task = task_spawn_cmd("ex_fixedwing_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20, SCHED_PRIORITY_MAX - 20,
2048, 2048,
fixedwing_control_thread_main, fixedwing_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
thread_running = true; thread_running = true;
exit(0); exit(0);
} }
src/examples/flow_position_estimator/flow_position_estimator_main.c
+82 -99
View File
@@ -81,8 +81,10 @@ static void usage(const char *reason);
static void usage(const char *reason) static void usage(const char *reason)
{ {
if (reason) if (reason) {
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1); exit(1);
} }
@@ -97,13 +99,12 @@ static void usage(const char *reason)
*/ */
int flow_position_estimator_main(int argc, char *argv[]) int flow_position_estimator_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
usage("missing command"); usage("missing command");
}
if (!strcmp(argv[1], "start")) if (!strcmp(argv[1], "start")) {
{ if (thread_running) {
if (thread_running)
{
printf("flow position estimator already running\n"); printf("flow position estimator already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
@@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
daemon_task = task_spawn_cmd("flow_position_estimator", daemon_task = task_spawn_cmd("flow_position_estimator",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
4000, 4000,
flow_position_estimator_thread_main, flow_position_estimator_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop")) {
{
thread_should_exit = true; thread_should_exit = true;
exit(0); exit(0);
} }
if (!strcmp(argv[1], "status")) if (!strcmp(argv[1], "status")) {
{ if (thread_running) {
if (thread_running)
printf("\tflow position estimator is running\n"); printf("\tflow position estimator is running\n");
else
} else {
printf("\tflow position estimator not started\n"); printf("\tflow position estimator not started\n");
}
exit(0); 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 */ /* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 },
{ 1, 0, 0 }, { 1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated { 0, 0, 1 }
const float time_scale = powf(10.0f,-6.0f); }; // 90deg rotated
const float time_scale = powf(10.0f, -6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f}; static float speed[3] = {0.0f, 0.0f, 0.0f};
static float flow_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}; 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 */ /* init local position and filtered flow struct */
struct vehicle_local_position_s local_pos = { struct vehicle_local_position_s local_pos = {
.x = 0.0f, .x = 0.0f,
.y = 0.0f, .y = 0.0f,
.z = 0.0f, .z = 0.0f,
.vx = 0.0f, .vx = 0.0f,
.vy = 0.0f, .vy = 0.0f,
.vz = 0.0f .vz = 0.0f
}; };
struct filtered_bottom_flow_s filtered_flow = { struct filtered_bottom_flow_s filtered_flow = {
.sumx = 0.0f, .sumx = 0.0f,
.sumy = 0.0f, .sumy = 0.0f,
.vx = 0.0f, .vx = 0.0f,
.vy = 0.0f .vy = 0.0f
}; };
/* advert pub messages */ /* 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_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); 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 */ /*This runs at the rate of the sensors */
struct pollfd fds[2] = { struct pollfd fds[2] = {
{ .fd = optical_flow_sub, .events = POLLIN }, { .fd = optical_flow_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN } { .fd = parameter_update_sub, .events = POLLIN }
}; };
/* wait for a sensor update, check for exit condition every 500 ms */ /* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500); int ret = poll(fds, 2, 500);
if (ret < 0) if (ret < 0) {
{
/* poll error, count it in perf */ /* poll error, count it in perf */
perf_count(mc_err_perf); perf_count(mc_err_perf);
} } else if (ret == 0) {
else if (ret == 0)
{
/* no return value, ignore */ /* no return value, ignore */
// printf("[flow position estimator] no bottom flow.\n"); // printf("[flow position estimator] no bottom flow.\n");
} } else {
else
{
/* parameter update available? */ /* parameter update available? */
if (fds[1].revents & POLLIN) if (fds[1].revents & POLLIN) {
{
/* read from param to clear updated flag */ /* read from param to clear updated flag */
struct parameter_update_s update; struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &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 */ /* only if flow data changed */
if (fds[0].revents & POLLIN) if (fds[0].revents & POLLIN) {
{
perf_begin(mc_loop_perf); perf_begin(mc_loop_perf);
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); 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 * -> minimum sonar value 0.3m
*/ */
if (!vehicle_liftoff) if (!vehicle_liftoff) {
{ if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) {
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
vehicle_liftoff = true; vehicle_liftoff = true;
} }
else
{ } else {
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) {
vehicle_liftoff = false; vehicle_liftoff = false;
}
} }
/* calc dt between flow timestamps */ /* calc dt between flow timestamps */
/* ignore first flow msg */ /* ignore first flow msg */
if(time_last_flow == 0) if (time_last_flow == 0) {
{
time_last_flow = flow.timestamp; time_last_flow = flow.timestamp;
continue; continue;
} }
dt = (float)(flow.timestamp - time_last_flow) * time_scale ; dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
time_last_flow = flow.timestamp; time_last_flow = flow.timestamp;
/* only make position update if vehicle is lift off or DEBUG is activated*/ /* 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 */ /* copy flow */
if (flow.integration_timespan > 0) { if (flow.integration_timespan > 0) {
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; 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; flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
} else { } else {
flow_speed[0] = 0; flow_speed[0] = 0;
flow_speed[1] = 0; flow_speed[1] = 0;
} }
flow_speed[2] = 0.0f; flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */ /* convert to bodyframe velocity */
for(uint8_t i = 0; i < 3; i++) for (uint8_t i = 0; i < 3; i++) {
{
float sum = 0.0f; 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]; sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];
}
speed[i] = sum; speed[i] = sum;
} }
@@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* convert to globalframe velocity /* convert to globalframe velocity
* -> local position is currently not used for position control * -> 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; 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); sum = sum + speed[j] * PX4_R(att.R, i, j);
}
global_speed[i] = sum; 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.vy = global_speed[1];
local_pos.xy_valid = true; local_pos.xy_valid = true;
local_pos.v_xy_valid = true; local_pos.v_xy_valid = true;
}
else } else {
{
/* set speed to zero and let position as it is */ /* set speed to zero and let position as it is */
filtered_flow.vx = 0; filtered_flow.vx = 0;
filtered_flow.vy = 0; filtered_flow.vy = 0;
@@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
} }
/* filtering ground distance */ /* filtering ground distance */
if (!vehicle_liftoff || !armed.armed) if (!vehicle_liftoff || !armed.armed) {
{
/* not possible to fly */ /* not possible to fly */
sonar_valid = false; sonar_valid = false;
local_pos.z = 0.0f; local_pos.z = 0.0f;
local_pos.z_valid = false; local_pos.z_valid = false;
}
else } else {
{
sonar_valid = true; sonar_valid = true;
} }
if (sonar_valid || params.debug) if (sonar_valid || params.debug) {
{
/* simple lowpass sonar filtering */ /* simple lowpass sonar filtering */
/* if new value or with sonar update frequency */ /* 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_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
sonar_last = sonar_new; 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; float height_diff = sonar_new - sonar_lp;
/* if over 1/2m spike follow lowpass */ /* 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; local_pos.z = -sonar_lp;
}
else } else {
{
local_pos.z = -sonar_new; 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(); local_pos.timestamp = hrt_absolute_time();
/* publish local position */ /* publish local position */
if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
&& isfinite(local_pos.vx) && isfinite(local_pos.vy)) && isfinite(local_pos.vx) && isfinite(local_pos.vy)) {
{
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
} }
/* publish filtered flow */ /* 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); 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 */ /* sensors not ready waiting for first attitude msg */
/* polling */ /* 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 */ /* wait for a attitude message, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000); int ret = poll(fds, 1, 5000);
if (ret < 0) if (ret < 0) {
{
/* poll error, count it in perf */ /* poll error, count it in perf */
perf_count(mc_err_perf); perf_count(mc_err_perf);
}
else if (ret == 0) } else if (ret == 0) {
{
/* no return value, ignore */ /* no return value, ignore */
printf("[flow position estimator] no attitude received.\n"); printf("[flow position estimator] no attitude received.\n");
}
else } else {
{ if (fds[0].revents & POLLIN) {
if (fds[0].revents & POLLIN){
sensors_ready = true; sensors_ready = true;
printf("[flow position estimator] initialized.\n"); printf("[flow position estimator] initialized.\n");
} }
src/examples/matlab_csv_serial/matlab_csv_serial.c
+25 -30
View File
@@ -73,8 +73,10 @@ static void usage(const char *reason);
static void usage(const char *reason) static void usage(const char *reason)
{ {
if (reason) if (reason) {
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1); exit(1);
} }
@@ -89,13 +91,12 @@ static void usage(const char *reason)
*/ */
int matlab_csv_serial_main(int argc, char *argv[]) int matlab_csv_serial_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
usage("missing command"); usage("missing command");
}
if (!strcmp(argv[1], "start")) if (!strcmp(argv[1], "start")) {
{ if (thread_running) {
if (thread_running)
{
warnx("already running\n"); warnx("already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
@@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
daemon_task = task_spawn_cmd("matlab_csv_serial", daemon_task = task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2000, 2000,
matlab_csv_serial_thread_main, matlab_csv_serial_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop")) {
{
thread_should_exit = true; thread_should_exit = true;
exit(0); exit(0);
} }
if (!strcmp(argv[1], "status")) if (!strcmp(argv[1], "status")) {
{
if (thread_running) { if (thread_running) {
warnx("running"); warnx("running");
} else { } else {
warnx("stopped"); 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"); 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); warnx("opening port %s", uart_name);
@@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
thread_running = true; thread_running = true;
while (!thread_should_exit) while (!thread_should_exit) {
{
/*This runs at the rate of the sensors */ /*This runs at the rate of the sensors */
struct pollfd fds[] = { 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 */ /* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
if (ret < 0) if (ret < 0) {
{
/* poll error, ignore */ /* poll error, ignore */
} } else if (ret == 0) {
else if (ret == 0)
{
/* no return value, ignore */ /* no return value, ignore */
warnx("no sensor data"); warnx("no sensor data");
}
else } else {
{
/* accel0 update available? */ /* 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), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
// write out on accel 0, but collect for all other sensors as they have updates // 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); (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
} }
src/examples/publisher/publisher_example.h
+5 -4
View File
@@ -40,7 +40,8 @@
#pragma once #pragma once
#include <px4.h> #include <px4.h>
class PublisherExample { class PublisherExample
{
public: public:
PublisherExample(); PublisherExample();
@@ -49,7 +50,7 @@ public:
int main(); int main();
protected: protected:
px4::NodeHandle _n; px4::NodeHandle _n;
px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; px4::Publisher<px4::px4_rc_channels> *_rc_channels_pub;
px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub; px4::Publisher<px4::px4_vehicle_attitude> *_v_att_pub;
px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub; px4::Publisher<px4::px4_parameter_update> *_parameter_update_pub;
}; };
src/examples/publisher/publisher_start_nuttx.cpp
+5 -5
View File
@@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[])
task_should_exit = false; task_should_exit = false;
daemon_task = task_spawn_cmd("publisher", daemon_task = task_spawn_cmd("publisher",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2000, 2000,
main, main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0); exit(0);
} }
src/examples/px4_daemon_app/px4_daemon_app.c
+14 -8
View File
@@ -71,8 +71,10 @@ static void usage(const char *reason);
static void static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) if (reason) {
warnx("%s\n", reason); warnx("%s\n", reason);
}
errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
} }
@@ -86,8 +88,9 @@ usage(const char *reason)
*/ */
int px4_daemon_app_main(int argc, char *argv[]) int px4_daemon_app_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
usage("missing command"); usage("missing command");
}
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
@@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
daemon_task = task_spawn_cmd("daemon", daemon_task = task_spawn_cmd("daemon",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2000, 2000,
px4_daemon_thread_main, px4_daemon_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0); exit(0);
} }
@@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
warnx("\trunning\n"); warnx("\trunning\n");
} else { } else {
warnx("\tnot started\n"); warnx("\tnot started\n");
} }
exit(0); exit(0);
} }
@@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[])
exit(1); exit(1);
} }
int px4_daemon_thread_main(int argc, char *argv[]) { int px4_daemon_thread_main(int argc, char *argv[])
{
warnx("[daemon] starting\n"); warnx("[daemon] starting\n");
src/examples/px4_simple_app/px4_simple_app.c
+8 -4
View File
@@ -80,14 +80,17 @@ int px4_simple_app_main(int argc, char *argv[])
if (poll_ret == 0) { if (poll_ret == 0) {
/* this means none of our providers is giving us data */ /* this means none of our providers is giving us data */
printf("[px4_simple_app] Got no data within a second\n"); printf("[px4_simple_app] Got no data within a second\n");
} else if (poll_ret < 0) { } else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */ /* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) { if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */ /* use a counter to prevent flooding (and slowing us down) */
printf("[px4_simple_app] ERROR return value from poll(): %d\n" printf("[px4_simple_app] ERROR return value from poll(): %d\n"
, poll_ret); , poll_ret);
} }
error_counter++; error_counter++;
} else { } else {
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
@@ -96,9 +99,9 @@ int px4_simple_app_main(int argc, char *argv[])
/* copy sensors raw data into local buffer */ /* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); 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", 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[0],
(double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]); (double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */ /* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0]; 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]; att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} }
/* there could be more file descriptors here, in the form like: /* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {} * if (fds[1..n].revents & POLLIN) {}
*/ */
src/examples/rover_steering_control/main.cpp
+11 -1
View File
@@ -267,18 +267,27 @@ int rover_steering_control_thread_main(int argc, char *argv[])
/* subscribe to topics. */ /* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int param_sub = orb_subscribe(ORB_ID(parameter_update)); int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */ /* Setup of loop */
struct pollfd fds[2]; struct pollfd fds[2];
fds[0].fd = param_sub; fds[0].fd = param_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
fds[1].fd = att_sub; fds[1].fd = att_sub;
fds[1].events = POLLIN; fds[1].events = POLLIN;
while (!thread_should_exit) { 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++) { for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) {
actuators.control[i] = 0.0f; actuators.control[i] = 0.0f;
} }
actuators.timestamp = hrt_absolute_time(); actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); 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, SCHED_PRIORITY_MAX - 20,
2048, 2048,
rover_steering_control_thread_main, rover_steering_control_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
thread_running = true; thread_running = true;
exit(0); exit(0);
} }
src/examples/subscriber/subscriber_example.cpp
+12 -8
View File
@@ -43,7 +43,8 @@
using namespace px4; 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); 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. * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
* Also the current value of the _sub_rc_chan subscription is printed * 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]", 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]", 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]", 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]", PX4_INFO("parameter_update_callback (method): [%llu]",
msg.data().timestamp); msg.data().timestamp);
_p_sub_interv.update(); _p_sub_interv.update();
PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get());
_p_test_float.update(); _p_test_float.update();
src/examples/subscriber/subscriber_example.h
+3 -2
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);
class SubscriberExample { class SubscriberExample
{
public: public:
SubscriberExample(); SubscriberExample();
@@ -54,7 +55,7 @@ protected:
px4::NodeHandle _n; px4::NodeHandle _n;
px4::ParameterInt _p_sub_interv; px4::ParameterInt _p_sub_interv;
px4::ParameterFloat _p_test_float; 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); void rc_channels_callback(const px4_rc_channels &msg);
src/examples/subscriber/subscriber_start_nuttx.cpp
+5 -5
View File
@@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[])
task_should_exit = false; task_should_exit = false;
daemon_task = task_spawn_cmd("subscriber", daemon_task = task_spawn_cmd("subscriber",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
2000, 2000,
main, main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL); (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
exit(0); exit(0);
} }