diff options
author | Julian Oes <julian@oes.ch> | 2013-06-15 20:06:30 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-06-15 20:06:30 +0200 |
commit | 3230f2244685252a362f3933707407022d8ea759 (patch) | |
tree | c1455c20f936becea7fc0a2262d206cc6560c055 /src/examples/flow_position_estimator/flow_position_estimator_main.c | |
parent | 9f5565de3221718ba12800a54ca1a0c06b7491ef (diff) | |
parent | 8559315f4f8b6515f2ba9ceadf2aa3b73af41bdc (diff) | |
download | px4-firmware-3230f2244685252a362f3933707407022d8ea759.tar.gz px4-firmware-3230f2244685252a362f3933707407022d8ea759.tar.bz2 px4-firmware-3230f2244685252a362f3933707407022d8ea759.zip |
Merge branch 'pid_fixes' into new_state_machine
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.c | 458 |
1 files changed, 458 insertions, 0 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 new file mode 100644 index 000000000..c0b16d2e7 --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -0,0 +1,458 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flow_position_estimator_main.c + * + * Optical flow position estimator + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/perf_counter.h> +#include <poll.h> + +#include "flow_position_estimator_params.h" + +__EXPORT int flow_position_estimator_main(int argc, char *argv[]); +static bool thread_should_exit = false; /**< Daemon exit flag */ +static bool thread_running = false; /**< Daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +int flow_position_estimator_thread_main(int argc, char *argv[]); +static void usage(const char *reason); + +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * 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 flow_position_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position estimator already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn("flow_position_estimator", + SCHED_RR, + SCHED_PRIORITY_MAX - 5, + 4096, + flow_position_estimator_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow position estimator is running\n"); + else + printf("\tflow position estimator not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int flow_position_estimator_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position estimator] starting\n"); + + /* 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); + 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}; + static uint32_t counter = 0; + static uint64_t time_last_flow = 0; // in ms + static float dt = 0.0f; // seconds + static float sonar_last = 0.0f; + static bool sonar_valid = false; + static float sonar_lp = 0.0f; + + /* subscribe to vehicle status, attitude, sensors and flow*/ + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + /* subscribe to parameter changes */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to vehicle status */ + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* subscribe to attitude setpoint */ + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + + /* subscribe to optical flow*/ + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); + + /* 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 + }; + struct filtered_bottom_flow_s filtered_flow = { + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f + }; + + /* advert pub messages */ + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow); + + /* vehicle flying status parameters */ + bool vehicle_liftoff = false; + bool sensors_ready = false; + + /* parameters init*/ + struct flow_position_estimator_params params; + struct flow_position_estimator_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime"); + 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) + { + 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 } + }; + + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow position estimator] no bottom flow.\n"); + } + else + { + + /* parameter update available? */ + 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); + + parameters_update(¶m_handles, ¶ms); + printf("[flow position estimator] parameters updated.\n"); + } + + /* only if flow data changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* got flow, updating attitude and status as well */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /* vehicle state estimation */ + float sonar_new = flow.ground_distance_m; + + /* set liftoff boolean + * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m) + * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) + * -> minimum sonar value 0.3m + */ + if (!vehicle_liftoff) + { + if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + vehicle_liftoff = true; + } + else + { + if (!vstatus.flag_system_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) + { + 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) + { + /* copy flow */ + flow_speed[0] = flow.flow_comp_x_m; + flow_speed[1] = flow.flow_comp_y_m; + flow_speed[2] = 0.0f; + + /* convert to bodyframe velocity */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + + speed[i] = sum; + } + + /* update filtered flow */ + filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt; + filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt; + filtered_flow.vx = speed[0]; + filtered_flow.vy = speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + /* convert to globalframe velocity + * -> local position is currently not used for position control + */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + speed[j] * att.R[i][j]; + + global_speed[i] = sum; + } + + local_pos.x = local_pos.x + global_speed[0] * dt; + local_pos.y = local_pos.y + global_speed[1] * dt; + local_pos.vx = global_speed[0]; + local_pos.vy = global_speed[1]; + } + else + { + /* set speed to zero and let position as it is */ + filtered_flow.vx = 0; + filtered_flow.vy = 0; + local_pos.vx = 0; + local_pos.vy = 0; + } + + /* filtering ground distance */ + if (!vehicle_liftoff || !vstatus.flag_system_armed) + { + /* not possible to fly */ + sonar_valid = false; + local_pos.z = 0.0f; + } + else + { + sonar_valid = true; + } + + 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) + { + sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; + sonar_last = sonar_new; + } + + 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) + { + local_pos.z = -sonar_lp; + } + else + { + local_pos.z = -sonar_new; + } + } + + filtered_flow.timestamp = hrt_absolute_time(); + 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)) + { + 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)) + { + orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); + } + + /* measure in what intervals the position estimator runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + + } + } + + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a attitude message, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow position estimator] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN){ + sensors_ready = true; + printf("[flow position estimator] initialized.\n"); + } + } + } + + counter++; + } + + printf("[flow position estimator] exiting.\n"); + thread_running = false; + + close(vehicle_attitude_setpoint_sub); + close(vehicle_attitude_sub); + close(vehicle_status_sub); + close(parameter_update_sub); + close(optical_flow_sub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + + |