diff options
Diffstat (limited to 'src/examples/flow_position_control/flow_position_control_main.c')
| -rw-r--r-- | src/examples/flow_position_control/flow_position_control_main.c | 614 |
1 files changed, 0 insertions, 614 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 deleted file mode 100644 index 3125ce246..000000000 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ /dev/null @@ -1,614 +0,0 @@ -/**************************************************************************** - * - * 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/actuator_armed.h> -#include <uORB/topics/vehicle_control_mode.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 <mavlink/mavlink_log.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_cmd(). - */ -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_cmd("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; - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[fpc] started"); - - uint32_t counter = 0; - const float time_scale = powf(10.0f,-6.0f); - - /* structures */ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct filtered_bottom_flow_s filtered_flow; - memset(&filtered_flow, 0, sizeof(filtered_flow)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_bodyframe_speed_setpoint_s speed_sp; - memset(&speed_sp, 0, sizeof(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 armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - 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; - static bool status_changed = 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); - mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); - } - - /* 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(actuator_armed), armed_sub, &armed); - /* 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); - /* get a local copy of control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - if (control_mode.flag_control_velocity_enabled) - { - 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 - - if(status_changed == false) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); - - status_changed = true; - - /* 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; //manual.throttle; - 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 */ - //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); - if(status_changed == true) - mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); - - status_changed = false; - 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 */ - mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN) - { - sensors_ready = true; - mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); - } - } - } - } - - mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); - - thread_running = false; - - close(parameter_update_sub); - close(vehicle_attitude_sub); - close(vehicle_local_position_sub); - close(armed_sub); - close(control_mode_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; -} - |