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 | |
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')
22 files changed, 2251 insertions, 90 deletions
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 <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_control.c + * + * Optical flow position controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <math.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.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/manual_control_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <poll.h> + +#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 <additional params>]\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 <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_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 <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_control_params.h + * + * Parameters for position controller + */ + +#include <systemlib/param/param.h> + +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 <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; +} + + 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 <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_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 <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_params.h + * + * Parameters for position estimator + */ + +#include <systemlib/param/param.h> + +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 <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_speed_control.c + * + * Optical flow speed controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <math.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.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_bodyframe_speed_setpoint.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <poll.h> + +#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 <additional params>]\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 <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_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 <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_speed_control_params.h + * + * Parameters for speed controller + */ + +#include <systemlib/param/param.h> + +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/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8..a226757e0 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); } /* Roll (P) */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118c..79194e515 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); } diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 71c78f5b8..48ec3603f 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value } /* only run controller if attitude changed */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d3..d7e1a3adc 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,29 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - //float yaw_awu; - //float yaw_lim; float att_p; float att_i; float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; }; /** @@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +114,10 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -170,8 +135,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; +// static int sensor_delay; +// sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; @@ -190,10 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -204,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index deba1ac03..57aea726a 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -150,13 +150,11 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; +// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; @@ -166,9 +164,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; + float pitch_control_last = 0.0f; + float roll_control_last = 0.0f; + static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ @@ -176,39 +180,44 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); } - /* calculate current control outputs */ - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; } else { - pitch_control = 0.0f; + pitch_control = pitch_control_last; warnx("rej. NaN ctrl pitch"); } - /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; } else { - roll_control = 0.0f; + roll_control = roll_control_last; warnx("rej. NaN ctrl roll"); } diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9..d0e67d3ea 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, float diff_filter_factor, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -51,15 +51,17 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; - - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->diff_filter_factor = diff_filter_factor; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; + + pid->sp = 0.0f; + pid->error_previous_filtered = 0.0f; + pid->control_previous = 0.0f; + pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) { int ret = 0; @@ -98,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } + if (isfinite(diff_filter_factor)) { + pid->limit = diff_filter_factor; + + } else { + ret = 1; + } + return ret; } @@ -136,15 +145,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } + float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; +// d = (error_filtered - pid->error_previous_filtered) / dt; + d = pid->error_previous_filtered - error_filtered; + pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -180,6 +189,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->last_output = output; } + pid->control_previous = pid->last_output; + + // if (isfinite(error)) { // Why is this necessary? DEW + // pid->error_previous = error; + // } return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d668867..f89c36d75 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -59,16 +59,18 @@ typedef struct { float intmax; float sp; float integral; - float error_previous; + float error_previous_filtered; + float control_previous; float last_output; float limit; uint8_t mode; + float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 313fbf641..7dcb9cf93 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -104,6 +104,9 @@ ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_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); @@ -122,6 +125,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 <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 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 <stdint.h> +#include <stdbool.h> +#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 <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 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 |