aboutsummaryrefslogtreecommitdiff
path: root/src/examples/flow_position_estimator/flow_position_estimator_main.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/examples/flow_position_estimator/flow_position_estimator_main.c')
-rw-r--r--src/examples/flow_position_estimator/flow_position_estimator_main.c181
1 files changed, 82 insertions, 99 deletions
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");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 19:09:02 +0000