forked from Archive/PX4-Autopilot
Removed obsolete flow control example
This commit is contained in:
Lorenz Meier
parent
1ce1ece0bb
commit
577dc879d3
|
|
@ -1,613 +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/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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -1,124 +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_params.c
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "flow_position_control_params.h"
|
|
||||||
|
|
||||||
/* controller parameters */
|
|
||||||
|
|
||||||
// Position control P gain
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
|
|
||||||
// Position control D / damping gain
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
|
|
||||||
// Altitude control P gain
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
|
|
||||||
// Altitude control I (integrator) gain
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
|
|
||||||
// Altitude control D gain
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
|
|
||||||
// Altitude control rate limiter
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
|
|
||||||
// Altitude control minimum altitude
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
|
|
||||||
// Altitude control maximum altitude (higher than 1.5m is untested)
|
|
||||||
PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
|
|
||||||
// Altitude control feed forward throttle - adjust to the
|
|
||||||
// throttle position (0..1) where the copter hovers in manual flight
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
@ -1,100 +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_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);
|
|
||||||
|
|
@ -1,41 +0,0 @@
|
||||||
############################################################################
|
|
||||||
#
|
|
||||||
# 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
|
|
||||||
Loading…
Reference in New Issue