mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
mc_pos_control: replacement for multirotor_pos_control, rewritten to C++ and new mathlib
This commit is contained in:
Anton Babushkin
@@ -261,6 +261,30 @@ public:
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* element by element multiplication
|
||||
*/
|
||||
const Vector<N> emult(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] * v.data[i];
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* element by element division
|
||||
*/
|
||||
const Vector<N> edivide(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] / v.data[i];
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,25 @@
|
||||
/*
|
||||
* mc_pos_control_params.c
|
||||
*
|
||||
* Created on: 26.12.2013
|
||||
* Author: ton
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/* multicopter position controller parameters */
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.05f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
|
||||
@@ -32,11 +32,10 @@
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Build multirotor position control
|
||||
# Build multicopter position controller
|
||||
#
|
||||
|
||||
MODULE_COMMAND = multirotor_pos_control
|
||||
MODULE_COMMAND = mc_pos_control
|
||||
|
||||
SRCS = multirotor_pos_control.c \
|
||||
multirotor_pos_control_params.c \
|
||||
thrust_pid.c
|
||||
SRCS = mc_pos_control_main.cpp \
|
||||
mc_pos_control_params.c
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,106 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 multirotor_pos_control_params.c
|
||||
*
|
||||
* Parameters for multirotor_pos_control
|
||||
*/
|
||||
|
||||
#include "multirotor_pos_control_params.h"
|
||||
|
||||
/* controller parameters */
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
|
||||
|
||||
int parameters_init(struct multirotor_position_control_param_handles *h)
|
||||
{
|
||||
h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
|
||||
h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
|
||||
h->thr_min = param_find("MPC_THR_MIN");
|
||||
h->thr_max = param_find("MPC_THR_MAX");
|
||||
h->z_p = param_find("MPC_Z_P");
|
||||
h->z_vel_p = param_find("MPC_Z_VEL_P");
|
||||
h->z_vel_i = param_find("MPC_Z_VEL_I");
|
||||
h->z_vel_max = param_find("MPC_Z_VEL_MAX");
|
||||
h->z_ff = param_find("MPC_Z_FF");
|
||||
h->xy_p = param_find("MPC_XY_P");
|
||||
h->xy_vel_p = param_find("MPC_XY_VEL_P");
|
||||
h->xy_vel_i = param_find("MPC_XY_VEL_I");
|
||||
h->xy_vel_max = param_find("MPC_XY_VEL_MAX");
|
||||
h->xy_ff = param_find("MPC_XY_FF");
|
||||
h->tilt_max = param_find("MPC_TILT_MAX");
|
||||
|
||||
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 multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
|
||||
{
|
||||
param_get(h->takeoff_alt, &(p->takeoff_alt));
|
||||
param_get(h->takeoff_gap, &(p->takeoff_gap));
|
||||
param_get(h->thr_min, &(p->thr_min));
|
||||
param_get(h->thr_max, &(p->thr_max));
|
||||
param_get(h->z_p, &(p->z_p));
|
||||
param_get(h->z_vel_p, &(p->z_vel_p));
|
||||
param_get(h->z_vel_i, &(p->z_vel_i));
|
||||
param_get(h->z_vel_max, &(p->z_vel_max));
|
||||
param_get(h->z_ff, &(p->z_ff));
|
||||
param_get(h->xy_p, &(p->xy_p));
|
||||
param_get(h->xy_vel_p, &(p->xy_vel_p));
|
||||
param_get(h->xy_vel_i, &(p->xy_vel_i));
|
||||
param_get(h->xy_vel_max, &(p->xy_vel_max));
|
||||
param_get(h->xy_ff, &(p->xy_ff));
|
||||
param_get(h->tilt_max, &(p->tilt_max));
|
||||
|
||||
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,97 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 multirotor_pos_control_params.h
|
||||
*
|
||||
* Parameters for multirotor_pos_control
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct multirotor_position_control_params {
|
||||
float takeoff_alt;
|
||||
float takeoff_gap;
|
||||
float thr_min;
|
||||
float thr_max;
|
||||
float z_p;
|
||||
float z_vel_p;
|
||||
float z_vel_i;
|
||||
float z_vel_max;
|
||||
float z_ff;
|
||||
float xy_p;
|
||||
float xy_vel_p;
|
||||
float xy_vel_i;
|
||||
float xy_vel_max;
|
||||
float xy_ff;
|
||||
float tilt_max;
|
||||
|
||||
float rc_scale_pitch;
|
||||
float rc_scale_roll;
|
||||
float rc_scale_yaw;
|
||||
};
|
||||
|
||||
struct multirotor_position_control_param_handles {
|
||||
param_t takeoff_alt;
|
||||
param_t takeoff_gap;
|
||||
param_t thr_min;
|
||||
param_t thr_max;
|
||||
param_t z_p;
|
||||
param_t z_vel_p;
|
||||
param_t z_vel_i;
|
||||
param_t z_vel_max;
|
||||
param_t z_ff;
|
||||
param_t xy_p;
|
||||
param_t xy_vel_p;
|
||||
param_t xy_vel_i;
|
||||
param_t xy_vel_max;
|
||||
param_t xy_ff;
|
||||
param_t tilt_max;
|
||||
|
||||
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 multirotor_position_control_param_handles *h);
|
||||
|
||||
/**
|
||||
* Update all parameters
|
||||
*
|
||||
*/
|
||||
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
|
||||
@@ -1,172 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 thrust_pid.c
|
||||
*
|
||||
* Implementation of thrust control PID.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include "thrust_pid.h"
|
||||
#include <math.h>
|
||||
|
||||
#define COS_TILT_MAX 0.7f
|
||||
|
||||
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min)
|
||||
{
|
||||
pid->dt_min = dt_min;
|
||||
pid->kp = 0.0f;
|
||||
pid->ki = 0.0f;
|
||||
pid->kd = 0.0f;
|
||||
pid->integral = 0.0f;
|
||||
pid->output_min = 0.0f;
|
||||
pid->output_max = 0.0f;
|
||||
pid->error_previous = 0.0f;
|
||||
pid->last_output = 0.0f;
|
||||
}
|
||||
|
||||
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if (isfinite(kp)) {
|
||||
pid->kp = kp;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (isfinite(ki)) {
|
||||
pid->ki = ki;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (isfinite(kd)) {
|
||||
pid->kd = kd;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (isfinite(output_min)) {
|
||||
pid->output_min = output_min;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (isfinite(output_max)) {
|
||||
pid->output_max = output_max;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
|
||||
{
|
||||
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
|
||||
return pid->last_output;
|
||||
}
|
||||
|
||||
float i, d;
|
||||
|
||||
/* error value */
|
||||
float error = sp - val;
|
||||
|
||||
/* error derivative */
|
||||
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
|
||||
pid->error_previous = -val;
|
||||
|
||||
if (!isfinite(d)) {
|
||||
d = 0.0f;
|
||||
}
|
||||
|
||||
/* calculate the error integral */
|
||||
i = pid->integral + (pid->ki * error * dt);
|
||||
|
||||
/* attitude-thrust compensation
|
||||
* r22 is (2, 2) component of rotation matrix for current attitude */
|
||||
float att_comp;
|
||||
|
||||
if (r22 > COS_TILT_MAX) {
|
||||
att_comp = 1.0f / r22;
|
||||
|
||||
} else if (r22 > 0.0f) {
|
||||
att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f;
|
||||
|
||||
} else {
|
||||
att_comp = 1.0f;
|
||||
}
|
||||
|
||||
/* calculate PD output */
|
||||
float output = ((error * pid->kp) + (d * pid->kd)) * att_comp;
|
||||
|
||||
/* check for saturation */
|
||||
if (isfinite(i)) {
|
||||
float i_comp = i * att_comp;
|
||||
if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) {
|
||||
/* not saturated, use new integral value */
|
||||
pid->integral = i;
|
||||
}
|
||||
}
|
||||
|
||||
/* add I component to output */
|
||||
output += pid->integral * att_comp;
|
||||
|
||||
/* limit output */
|
||||
if (isfinite(output)) {
|
||||
if (output > pid->output_max) {
|
||||
output = pid->output_max;
|
||||
|
||||
} else if (output < pid->output_min) {
|
||||
output = pid->output_min;
|
||||
}
|
||||
|
||||
pid->last_output = output;
|
||||
}
|
||||
|
||||
return pid->last_output;
|
||||
}
|
||||
|
||||
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
|
||||
{
|
||||
pid->integral = i;
|
||||
}
|
||||
@@ -1,69 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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 thrust_pid.h
|
||||
*
|
||||
* Definition of thrust control PID interface.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef THRUST_PID_H_
|
||||
#define THRUST_PID_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
typedef struct {
|
||||
float dt_min;
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
float integral;
|
||||
float output_min;
|
||||
float output_max;
|
||||
float error_previous;
|
||||
float last_output;
|
||||
} thrust_pid_t;
|
||||
|
||||
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min);
|
||||
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max);
|
||||
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
|
||||
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif /* THRUST_PID_H_ */
|
||||
Reference in New Issue
Block a user