From f28cec350cb79139b6cc9d9cff32954a644e6f07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Jun 2013 12:44:11 +0200 Subject: Hotfix: Excluded sdlog app from standard build, still keeping code in place for now --- makefiles/config_px4fmu_default.mk | 2 -- 1 file changed, 2 deletions(-) (limited to 'makefiles') diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index d50eb1e50..0bd6d4bd8 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -63,7 +63,6 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3_comp -#MODULES += modules/position_estimator_mc MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf @@ -79,7 +78,6 @@ MODULES += modules/multirotor_pos_control # # Logging # -MODULES += modules/sdlog MODULES += modules/sdlog2 # -- cgit v1.2.3 From b789e01a0f396cd934dfc07d8a5834333aabf51e Mon Sep 17 00:00:00 2001 From: samuezih Date: Fri, 14 Jun 2013 17:29:19 +0200 Subject: Add PX4Flow board modules and corresponding ORB msgs. --- makefiles/config_px4fmu_default.mk | 3 + .../flow_position_control_main.c | 589 +++++++++++++++++++++ .../flow_position_control_params.c | 113 ++++ .../flow_position_control_params.h | 100 ++++ src/examples/flow_position_control/module.mk | 41 ++ .../flow_position_estimator_main.c | 458 ++++++++++++++++ .../flow_position_estimator_params.c | 72 +++ .../flow_position_estimator_params.h | 68 +++ src/examples/flow_position_estimator/module.mk | 41 ++ .../flow_speed_control/flow_speed_control_main.c | 361 +++++++++++++ .../flow_speed_control/flow_speed_control_params.c | 70 +++ .../flow_speed_control/flow_speed_control_params.h | 70 +++ src/examples/flow_speed_control/module.mk | 41 ++ src/modules/uORB/objects_common.cpp | 6 + src/modules/uORB/topics/filtered_bottom_flow.h | 74 +++ .../uORB/topics/vehicle_bodyframe_speed_setpoint.h | 69 +++ 16 files changed, 2176 insertions(+) create mode 100644 src/examples/flow_position_control/flow_position_control_main.c create mode 100644 src/examples/flow_position_control/flow_position_control_params.c create mode 100644 src/examples/flow_position_control/flow_position_control_params.h create mode 100644 src/examples/flow_position_control/module.mk create mode 100644 src/examples/flow_position_estimator/flow_position_estimator_main.c create mode 100644 src/examples/flow_position_estimator/flow_position_estimator_params.c create mode 100644 src/examples/flow_position_estimator/flow_position_estimator_params.h create mode 100644 src/examples/flow_position_estimator/module.mk create mode 100644 src/examples/flow_speed_control/flow_speed_control_main.c create mode 100644 src/examples/flow_speed_control/flow_speed_control_params.c create mode 100644 src/examples/flow_speed_control/flow_speed_control_params.h create mode 100644 src/examples/flow_speed_control/module.mk create mode 100644 src/modules/uORB/topics/filtered_bottom_flow.h create mode 100644 src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h (limited to 'makefiles') diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 0bd6d4bd8..43288537c 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -65,6 +65,7 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3_comp MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += examples/flow_position_estimator # # Vehicle Control @@ -74,6 +75,8 @@ MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control +MODULES += examples/flow_position_control +MODULES += examples/flow_speed_control # # Logging diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c new file mode 100644 index 000000000..c177c8fd2 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -0,0 +1,589 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_control.c + * + * Optical flow position controller + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flow_position_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_position_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_position_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon 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_spawn(). + */ +int flow_position_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_position_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_position_control_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 control app is running\n"); + else + printf("\tflow position control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_position_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position control] starting\n"); + + uint32_t counter = 0; + const float time_scale = powf(10.0f,-6.0f); + + /* structures */ + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + struct manual_control_setpoint_s manual; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_local_position_s local_pos; + + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + + orb_advert_t speed_sp_pub; + bool speed_setpoint_adverted = false; + + /* parameters init*/ + struct flow_position_control_params params; + struct flow_position_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* init flow sum setpoint */ + float flow_sp_sumx = 0.0f; + float flow_sp_sumy = 0.0f; + + /* init yaw setpoint */ + float yaw_sp = 0.0f; + + /* init height setpoint */ + float height_sp = params.height_min; + + /* height controller states */ + bool start_phase = true; + bool landing_initialized = false; + float landing_thrust_start = 0.0f; + + /* states */ + float integrated_h_error = 0.0f; + float last_local_pos_z = 0.0f; + bool update_flow_sp_sumx = false; + bool update_flow_sp_sumy = false; + uint64_t last_time = 0.0f; + float dt = 0.0f; // s + + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator + { .fd = parameter_update_sub, .events = POLLIN } + + }; + + /* wait for a position 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 control] no filtered flow updates\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 control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 + float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 + float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + + /* calc dt */ + if(last_time == 0) + { + last_time = hrt_absolute_time(); + continue; + } + dt = ((float) (hrt_absolute_time() - last_time)) * time_scale; + last_time = hrt_absolute_time(); + + /* update flow sum setpoint */ + if (update_flow_sp_sumx) + { + flow_sp_sumx = filtered_flow.sumx; + update_flow_sp_sumx = false; + } + if (update_flow_sp_sumy) + { + flow_sp_sumy = filtered_flow.sumy; + update_flow_sp_sumy = false; + } + + /* calc new bodyframe speed setpoints */ + float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; + float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d; + float speed_limit_height_factor = height_sp; // the settings are for 1 meter + + /* overwrite with rc input if there is any */ + if(isfinite(manual_pitch) && isfinite(manual_roll)) + { + if(fabsf(manual_pitch) > params.manual_threshold) + { + speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor; + update_flow_sp_sumx = true; + } + + if(fabsf(manual_roll) > params.manual_threshold) + { + speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor; + update_flow_sp_sumy = true; + } + } + + /* limit speed setpoints */ + if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) && + (speed_body_x >= -params.limit_speed_x * speed_limit_height_factor)) + { + speed_sp.vx = speed_body_x; + } + else + { + if(speed_body_x > params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = params.limit_speed_x * speed_limit_height_factor; + if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor; + } + + if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) && + (speed_body_y >= -params.limit_speed_y * speed_limit_height_factor)) + { + speed_sp.vy = speed_body_y; + } + else + { + if(speed_body_y > params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = params.limit_speed_y * speed_limit_height_factor; + if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor; + } + + /* manual yaw change */ + if(isfinite(manual_yaw) && isfinite(manual.throttle)) + { + if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f) + { + yaw_sp += manual_yaw * params.limit_yaw_step; + + /* modulo for rotation -pi +pi */ + if(yaw_sp < -M_PI_F) + yaw_sp = yaw_sp + M_TWOPI_F; + else if(yaw_sp > M_PI_F) + yaw_sp = yaw_sp - M_TWOPI_F; + } + } + + /* forward yaw setpoint */ + speed_sp.yaw_sp = yaw_sp; + + + /* manual height control + * 0-20%: thrust linear down + * 20%-40%: down + * 40%-60%: stabilize altitude + * 60-100%: up + */ + float thrust_control = 0.0f; + + if (isfinite(manual.throttle)) + { + if (start_phase) + { + /* control start thrust with stick input */ + if (manual.throttle < 0.4f) + { + /* first 40% for up to feedforward */ + thrust_control = manual.throttle / 0.4f * params.thrust_feedforward; + } + else + { + /* second 60% for up to feedforward + 10% */ + thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward; + } + + /* exit start phase if setpoint is reached */ + if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower) + { + start_phase = false; + /* switch to stabilize */ + thrust_control = params.thrust_feedforward; + } + } + else + { + if (manual.throttle < 0.2f) + { + /* landing initialization */ + if (!landing_initialized) + { + /* consider last thrust control to avoid steps */ + landing_thrust_start = speed_sp.thrust_sp; + landing_initialized = true; + } + + /* set current height as setpoint to avoid steps */ + if (-local_pos.z > params.height_min) + height_sp = -local_pos.z; + else + height_sp = params.height_min; + + /* lower 20% stick range controls thrust down */ + thrust_control = manual.throttle / 0.2f * landing_thrust_start; + + /* assume ground position here */ + if (thrust_control < 0.1f) + { + /* reset integral if on ground */ + integrated_h_error = 0.0f; + /* switch to start phase */ + start_phase = true; + /* reset height setpoint */ + height_sp = params.height_min; + } + } + else + { + /* stabilized mode */ + landing_initialized = false; + + /* calc new thrust with PID */ + float height_error = (local_pos.z - (-height_sp)); + + /* update height setpoint if needed*/ + if (manual.throttle < 0.4f) + { + /* down */ + if (height_sp > params.height_min + params.height_rate && + fabsf(height_error) < params.limit_height_error) + height_sp -= params.height_rate * dt; + } + + if (manual.throttle > 0.6f) + { + /* up */ + if (height_sp < params.height_max && + fabsf(height_error) < params.limit_height_error) + height_sp += params.height_rate * dt; + } + + /* instead of speed limitation, limit height error (downwards) */ + if(height_error > params.limit_height_error) + height_error = params.limit_height_error; + else if(height_error < -params.limit_height_error) + height_error = -params.limit_height_error; + + integrated_h_error = integrated_h_error + height_error; + float integrated_thrust_addition = integrated_h_error * params.height_i; + + if(integrated_thrust_addition > params.limit_thrust_int) + integrated_thrust_addition = params.limit_thrust_int; + if(integrated_thrust_addition < -params.limit_thrust_int) + integrated_thrust_addition = -params.limit_thrust_int; + + float height_speed = last_local_pos_z - local_pos.z; + float thrust_diff = height_error * params.height_p - height_speed * params.height_d; + + thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition; + + /* add attitude component + * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM + */ +// // TODO problem with attitude +// if (att.R_valid && att.R[2][2] > 0) +// thrust_control = thrust_control / att.R[2][2]; + + /* set thrust lower limit */ + if(thrust_control < params.limit_thrust_lower) + thrust_control = params.limit_thrust_lower; + } + } + + /* set thrust upper limit */ + if(thrust_control > params.limit_thrust_upper) + thrust_control = params.limit_thrust_upper; + } + /* store actual height for speed estimation */ + last_local_pos_z = local_pos.z; + + speed_sp.thrust_sp = thrust_control; + speed_sp.timestamp = hrt_absolute_time(); + + /* publish new speed setpoint */ + if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp)) + { + + if(speed_setpoint_adverted) + { + orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp); + } + else + { + speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp); + speed_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow position controller!"); + } + } + else + { + /* in manual or stabilized state just reset speed and flow sum setpoint */ + speed_sp.vx = 0.0f; + speed_sp.vy = 0.0f; + flow_sp_sumx = filtered_flow.sumx; + flow_sp_sumy = filtered_flow.sumy; + if(isfinite(att.yaw)) + { + yaw_sp = att.yaw; + speed_sp.yaw_sp = att.yaw; + } + if(isfinite(manual.throttle)) + speed_sp.thrust_sp = manual.throttle; + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, 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 control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow position control] initialized.\n"); + } + } + } + } + + printf("[flow position control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_local_position_sub); + close(vehicle_status_sub); + close(manual_control_setpoint_sub); + close(speed_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c new file mode 100644 index 000000000..4f3598a57 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_control_params.c + */ + +#include "flow_position_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); +PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); +PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); +PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); +PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); +PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); +PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); +PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); +PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight +PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f); +PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f); +PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f); +PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f); +PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f); +PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f); + + +int parameters_init(struct flow_position_control_param_handles *h) +{ + /* PID parameters */ + h->pos_p = param_find("FPC_POS_P"); + h->pos_d = param_find("FPC_POS_D"); + h->height_p = param_find("FPC_H_P"); + h->height_i = param_find("FPC_H_I"); + h->height_d = param_find("FPC_H_D"); + h->height_rate = param_find("FPC_H_RATE"); + h->height_min = param_find("FPC_H_MIN"); + h->height_max = param_find("FPC_H_MAX"); + h->thrust_feedforward = param_find("FPC_T_FFWD"); + h->limit_speed_x = param_find("FPC_L_S_X"); + h->limit_speed_y = param_find("FPC_L_S_Y"); + h->limit_height_error = param_find("FPC_L_H_ERR"); + h->limit_thrust_int = param_find("FPC_L_TH_I"); + h->limit_thrust_upper = param_find("FPC_L_TH_U"); + h->limit_thrust_lower = param_find("FPC_L_TH_L"); + h->limit_yaw_step = param_find("FPC_L_YAW_STEP"); + h->manual_threshold = param_find("FPC_MAN_THR"); + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); + + return OK; +} + +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p) +{ + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->height_p, &(p->height_p)); + param_get(h->height_i, &(p->height_i)); + param_get(h->height_d, &(p->height_d)); + param_get(h->height_rate, &(p->height_rate)); + param_get(h->height_min, &(p->height_min)); + param_get(h->height_max, &(p->height_max)); + param_get(h->thrust_feedforward, &(p->thrust_feedforward)); + param_get(h->limit_speed_x, &(p->limit_speed_x)); + param_get(h->limit_speed_y, &(p->limit_speed_y)); + param_get(h->limit_height_error, &(p->limit_height_error)); + param_get(h->limit_thrust_int, &(p->limit_thrust_int)); + param_get(h->limit_thrust_upper, &(p->limit_thrust_upper)); + param_get(h->limit_thrust_lower, &(p->limit_thrust_lower)); + param_get(h->limit_yaw_step, &(p->limit_yaw_step)); + param_get(h->manual_threshold, &(p->manual_threshold)); + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); + + return OK; +} diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h new file mode 100644 index 000000000..d0c8fc722 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_control_params.h + * + * Parameters for position controller + */ + +#include + +struct flow_position_control_params { + float pos_p; + float pos_d; + float height_p; + float height_i; + float height_d; + float height_rate; + float height_min; + float height_max; + float thrust_feedforward; + float limit_speed_x; + float limit_speed_y; + float limit_height_error; + float limit_thrust_int; + float limit_thrust_upper; + float limit_thrust_lower; + float limit_yaw_step; + float manual_threshold; + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; +}; + +struct flow_position_control_param_handles { + param_t pos_p; + param_t pos_d; + param_t height_p; + param_t height_i; + param_t height_d; + param_t height_rate; + param_t height_min; + param_t height_max; + param_t thrust_feedforward; + param_t limit_speed_x; + param_t limit_speed_y; + param_t limit_height_error; + param_t limit_thrust_int; + param_t limit_thrust_upper; + param_t limit_thrust_lower; + param_t limit_yaw_step; + param_t manual_threshold; + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p); diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk new file mode 100644 index 000000000..b10dc490a --- /dev/null +++ b/src/examples/flow_position_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# 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. +# +############################################################################ + +# +# Build multirotor position control +# + +MODULE_COMMAND = flow_position_control + +SRCS = flow_position_control_main.c \ + flow_position_control_params.c 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 + * Lorenz Meier + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 ]\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; +} + + diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c new file mode 100644 index 000000000..ec3c3352d --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_params.c + * + * Parameters for position estimator + */ + +#include "flow_position_estimator_params.h" + +/* Extended Kalman Filter covariances */ + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f); +PARAM_DEFINE_INT32(FPE_DEBUG, 0); + + +int parameters_init(struct flow_position_estimator_param_handles *h) +{ + /* PID parameters */ + h->minimum_liftoff_thrust = param_find("FPE_LO_THRUST"); + h->sonar_upper_lp_threshold = param_find("FPE_SONAR_LP_U"); + h->sonar_lower_lp_threshold = param_find("FPE_SONAR_LP_L"); + h->debug = param_find("FPE_DEBUG"); + + return OK; +} + +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p) +{ + param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust)); + param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold)); + param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold)); + param_get(h->debug, &(p->debug)); + + return OK; +} diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h new file mode 100644 index 000000000..f9a9bb303 --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_params.h + * + * Parameters for position estimator + */ + +#include + +struct flow_position_estimator_params { + float minimum_liftoff_thrust; + float sonar_upper_lp_threshold; + float sonar_lower_lp_threshold; + int debug; +}; + +struct flow_position_estimator_param_handles { + param_t minimum_liftoff_thrust; + param_t sonar_upper_lp_threshold; + param_t sonar_lower_lp_threshold; + param_t debug; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_estimator_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p); diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk new file mode 100644 index 000000000..88c9ceb93 --- /dev/null +++ b/src/examples/flow_position_estimator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# 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. +# +############################################################################ + +# +# Build position estimator +# + +MODULE_COMMAND = flow_position_estimator + +SRCS = flow_position_estimator_main.c \ + flow_position_estimator_params.c diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c new file mode 100644 index 000000000..9648728c8 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_speed_control.c + * + * Optical flow speed controller + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flow_speed_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_speed_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_speed_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon 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_spawn(). + */ +int flow_speed_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow speed control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_speed_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_speed_control_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 speed control app is running\n"); + else + printf("\tflow speed control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_speed_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow speed control] starting\n"); + + uint32_t counter = 0; + + /* structures */ + struct vehicle_status_s vstatus; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + struct vehicle_attitude_setpoint_s att_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); + + orb_advert_t att_sp_pub; + bool attitude_setpoint_adverted = false; + + /* parameters init*/ + struct flow_speed_control_params params; + struct flow_speed_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller + { .fd = parameter_update_sub, .events = POLLIN } + }; + + /* wait for a position update, check for exit condition every 5000 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 speed control] no bodyframe speed setpoints updates\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 speed control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of bodyframe speed setpoint */ + orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); + + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + /* calc new roll/pitch */ + float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; + float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + + /* limit roll and pitch corrections */ + if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) + { + att_sp.pitch_body = pitch_body; + } + else + { + if(pitch_body > params.limit_pitch) + att_sp.pitch_body = params.limit_pitch; + if(pitch_body < -params.limit_pitch) + att_sp.pitch_body = -params.limit_pitch; + } + + if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll)) + { + att_sp.roll_body = roll_body; + } + else + { + if(roll_body > params.limit_roll) + att_sp.roll_body = params.limit_roll; + if(roll_body < -params.limit_roll) + att_sp.roll_body = -params.limit_roll; + } + + /* set yaw setpoint forward*/ + att_sp.yaw_body = speed_sp.yaw_sp; + + /* add trim from parameters */ + att_sp.roll_body = att_sp.roll_body + params.trim_roll; + att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch; + + att_sp.thrust = speed_sp.thrust_sp; + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust)) + { + if (attitude_setpoint_adverted) + { + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + else + { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + attitude_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow speed controller!"); + } + } + else + { + /* reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.0f; + att_sp.yaw_body = 0.0f; + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, 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 speed control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow speed control] initialized.\n"); + } + } + } + } + + printf("[flow speed control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_bodyframe_speed_setpoint_sub); + close(filtered_bottom_flow_sub); + close(vehicle_status_sub); + close(att_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c new file mode 100644 index 000000000..8dfe54173 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.c @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_speed_control_params.c + * + */ + +#include "flow_speed_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f); +PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f); + +int parameters_init(struct flow_speed_control_param_handles *h) +{ + /* PID parameters */ + h->speed_p = param_find("FSC_S_P"); + h->limit_pitch = param_find("FSC_L_PITCH"); + h->limit_roll = param_find("FSC_L_ROLL"); + h->trim_roll = param_find("TRIM_ROLL"); + h->trim_pitch = param_find("TRIM_PITCH"); + + + return OK; +} + +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p) +{ + param_get(h->speed_p, &(p->speed_p)); + param_get(h->limit_pitch, &(p->limit_pitch)); + param_get(h->limit_roll, &(p->limit_roll)); + param_get(h->trim_roll, &(p->trim_roll)); + param_get(h->trim_pitch, &(p->trim_pitch)); + + return OK; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h new file mode 100644 index 000000000..eec27a2bf --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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_speed_control_params.h + * + * Parameters for speed controller + */ + +#include + +struct flow_speed_control_params { + float speed_p; + float limit_pitch; + float limit_roll; + float trim_roll; + float trim_pitch; +}; + +struct flow_speed_control_param_handles { + param_t speed_p; + param_t limit_pitch; + param_t limit_roll; + param_t trim_roll; + param_t trim_pitch; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_speed_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p); diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk new file mode 100644 index 000000000..5a4182146 --- /dev/null +++ b/src/examples/flow_speed_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# 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. +# +############################################################################ + +# +# Build flow speed control +# + +MODULE_COMMAND = flow_speed_control + +SRCS = flow_speed_control_main.c \ + flow_speed_control_params.c diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2..e22b58cad 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -101,6 +101,9 @@ ORB_DEFINE(vehicle_command, struct vehicle_command_s); #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); +#include "topics/vehicle_bodyframe_speed_setpoint.h" +ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); + #include "topics/vehicle_global_position_setpoint.h" ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); @@ -119,6 +122,9 @@ ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); +#include "topics/filtered_bottom_flow.h" +ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); + #include "topics/omnidirectional_flow.h" ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h new file mode 100644 index 000000000..ab6de2613 --- /dev/null +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 filtered_bottom_flow.h + * Definition of the filtered bottom flow uORB topic. + */ + +#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ +#define TOPIC_FILTERED_BOTTOM_FLOW_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Filtered bottom flow in bodyframe. + */ +struct filtered_bottom_flow_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + + float sumx; /**< Integrated bodyframe x flow in meters */ + float sumy; /**< Integrated bodyframe y flow in meters */ + + float vx; /**< Flow bodyframe x speed, m/s */ + float vy; /**< Flow bodyframe y Speed, m/s */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(filtered_bottom_flow); + +#endif diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h new file mode 100644 index 000000000..fbfab09f3 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann + * Lorenz Meier + * + * 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 vehicle_bodyframe_speed_setpoint.h + * Definition of the bodyframe speed setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ +#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_bodyframe_speed_setpoint_s +{ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + float vx; /**< in m/s */ + float vy; /**< in m/s */ +// float vz; /**< in m/s */ + float thrust_sp; + float yaw_sp; /**< in radian -PI +PI */ +}; /**< Speed in bodyframe to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_bodyframe_speed_setpoint); + +#endif -- cgit v1.2.3