mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
examples fix code style
This commit is contained in:
committed by
Lorenz Meier
parent
0eaf41a048
commit
662ec3f62f
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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");
|
||||
|
||||
|
||||
@@ -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) {}
|
||||
*/
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user