mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
Adding twisting and state feedback controllers. Twisting is done on tilt axis only and works mostly. sfb is just a start.
This commit is contained in:
Paul Cox
@@ -238,9 +238,70 @@ main_overo.LDFLAGS += -levent -lm
|
||||
main_overo.srcs += $(SRC_BETH)/overo_gcs_com.c
|
||||
main_overo.srcs += $(SRC_BETH)/overo_file_logger.c
|
||||
main_overo.srcs += $(SRC_BETH)/overo_estimator.c
|
||||
main_overo.CFLAGS += -DCONTROLLER_H=\"overo_controller.h\"
|
||||
main_overo.srcs += $(SRC_BETH)/overo_controller.c
|
||||
|
||||
|
||||
#
|
||||
# Overo twisting
|
||||
#
|
||||
|
||||
USER =
|
||||
HOST = auto3
|
||||
TARGET_DIR = ~
|
||||
SRC_FMS=fms
|
||||
|
||||
overo_twist.ARCHDIR = omap
|
||||
overo_twist.CFLAGS = -I. -I$(SRC_FMS)
|
||||
overo_twist.srcs = $(SRC_BETH)/main_overo.c
|
||||
overo_twist.CFLAGS += -DFMS_PERIODIC_FREQ=512
|
||||
overo_twist.srcs += $(SRC_FMS)/fms_periodic.c
|
||||
overo_twist.srcs += $(SRC_FMS)/fms_serial_port.c
|
||||
overo_twist.LDFLAGS += -lrt
|
||||
overo_twist.srcs += $(SRC_FMS)/fms_spi_link.c
|
||||
overo_twist.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
|
||||
|
||||
overo_twist.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
|
||||
overo_twist.srcs += $(SRC_FMS)/udp_transport2.c downlink.c
|
||||
overo_twist.srcs += $(SRC_FMS)/fms_network.c
|
||||
overo_twist.LDFLAGS += -levent -lm
|
||||
|
||||
overo_twist.srcs += $(SRC_BETH)/overo_gcs_com.c
|
||||
overo_twist.srcs += $(SRC_BETH)/overo_file_logger.c
|
||||
overo_twist.srcs += $(SRC_BETH)/overo_estimator.c
|
||||
overo_twist.CFLAGS += -DCONTROLLER_H=\"overo_twist_controller.h\"
|
||||
overo_twist.srcs += $(SRC_BETH)/overo_twist_controller.c
|
||||
|
||||
#
|
||||
# Overo state feedback
|
||||
#
|
||||
|
||||
USER =
|
||||
HOST = auto3
|
||||
TARGET_DIR = ~
|
||||
SRC_FMS=fms
|
||||
|
||||
overo_sfb.ARCHDIR = omap
|
||||
overo_sfb.CFLAGS = -I. -I$(SRC_FMS)
|
||||
overo_sfb.srcs = $(SRC_BETH)/main_overo.c
|
||||
overo_sfb.CFLAGS += -DFMS_PERIODIC_FREQ=512
|
||||
overo_sfb.srcs += $(SRC_FMS)/fms_periodic.c
|
||||
overo_sfb.srcs += $(SRC_FMS)/fms_serial_port.c
|
||||
overo_sfb.LDFLAGS += -lrt
|
||||
overo_sfb.srcs += $(SRC_FMS)/fms_spi_link.c
|
||||
overo_sfb.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
|
||||
|
||||
overo_sfb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
|
||||
overo_sfb.srcs += $(SRC_FMS)/udp_transport2.c downlink.c
|
||||
overo_sfb.srcs += $(SRC_FMS)/fms_network.c
|
||||
overo_sfb.LDFLAGS += -levent -lm
|
||||
|
||||
overo_sfb.srcs += $(SRC_BETH)/overo_gcs_com.c
|
||||
overo_sfb.srcs += $(SRC_BETH)/overo_file_logger.c
|
||||
overo_sfb.srcs += $(SRC_BETH)/overo_estimator.c
|
||||
overo_sfb.CFLAGS += -DCONTROLLER_H=\"overo_sfb_controller.h\"
|
||||
overo_sfb.srcs += $(SRC_BETH)/overo_sfb_controller.c
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
@@ -4,15 +4,15 @@
|
||||
<dl_settings>
|
||||
<dl_settings NAME="Controller">
|
||||
|
||||
<dl_setting var="controller.elevation_sp" min="-25" step="1" max="20" module="beth/overo_controller" shortname="elev_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
|
||||
<dl_setting var="controller.elevation_sp" min="-25" step="1" max="20" module="beth/overo_twist_controller" shortname="elev_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
|
||||
|
||||
<dl_setting var="controller.azimuth_sp" min="-15" step="0.5" max="15" module="beth/overo_controller" shortname="azim_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578">
|
||||
<dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15" module="beth/overo_twist_controller" shortname="tilt_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578">
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting var="controller.azim_gain" min="0" step=".01" max=".1" module="beth/overo_estimator" shortname="azim_gain">
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting var="controller.omega_tilt_ref" min="200" step="100" max="1200" module="beth/overo_controller" shortname="tilt_omega_ref" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578">
|
||||
<dl_setting var="controller.omega_tilt_ref" min="200" step="100" max="1200" module="beth/overo_twist_controller" shortname="tilt_omega_ref" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578">
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting var="estimator.elevation_lp_coeff" min="0.01" step="0.01" max="1" module="beth/overo_estimator" shortname="elev_lp_coeff">
|
||||
|
||||
@@ -42,11 +42,12 @@
|
||||
#include "overo_file_logger.h"
|
||||
#include "overo_gcs_com.h"
|
||||
#include "overo_estimator.h"
|
||||
#include "overo_controller.h"
|
||||
#include CONTROLLER_H
|
||||
|
||||
|
||||
static void main_periodic(int);
|
||||
//static void main_parse_cmd_line(int argc, char *argv[]);
|
||||
static void drive_output(uint8_t last_state);
|
||||
static void main_exit(int sig);
|
||||
static void main_talk_with_stm32(void);
|
||||
|
||||
@@ -120,7 +121,7 @@ static void main_periodic(int my_sig_num) {
|
||||
|
||||
BoozImuScaleGyro(booz_imu);
|
||||
|
||||
RunOnceEvery(15, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,
|
||||
RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,
|
||||
&msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y,
|
||||
&msg_in.payload.msg_up.bench_sensor.z,&msg_in.payload.msg_up.cnt,
|
||||
&msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs,
|
||||
@@ -142,6 +143,60 @@ static void main_periodic(int my_sig_num) {
|
||||
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
|
||||
}
|
||||
|
||||
|
||||
drive_output(last_state);
|
||||
|
||||
control_send_messages();
|
||||
|
||||
estimator_send_messages();
|
||||
|
||||
//file_logger_periodic();
|
||||
|
||||
/* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
|
||||
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
|
||||
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);})
|
||||
RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
|
||||
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
|
||||
|
||||
RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
|
||||
&estimator.tilt, &estimator.elevation, &estimator.azimuth );});
|
||||
RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
|
||||
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
|
||||
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
|
||||
|
||||
RunOnceEvery(33, gcs_com_periodic());
|
||||
|
||||
}
|
||||
|
||||
#if 0
|
||||
static void main_parse_cmd_line(int argc, char *argv[]) {
|
||||
|
||||
if (argc>1){
|
||||
controller.kp = atof(argv[1]);
|
||||
// printf("kp set to %f\n",kp);
|
||||
if (argc>2) {
|
||||
controller.kd = atof(argv[2]);
|
||||
// printf("kd set to %f\n",kd);
|
||||
} else {
|
||||
controller.kd=1.0;
|
||||
// printf("using default value of kd %f\n",kd);
|
||||
}
|
||||
} else {
|
||||
controller.kp = 0.05;
|
||||
// printf("using default value of kp %f\n",kp);
|
||||
}
|
||||
/*
|
||||
if (argc>1){
|
||||
printf("args not currently supported\n");
|
||||
}*/
|
||||
}
|
||||
#endif
|
||||
|
||||
static void drive_output(uint8_t last_state) {
|
||||
switch (controller.armed) {
|
||||
case 0:
|
||||
if (last_state == 2) {
|
||||
@@ -187,73 +242,8 @@ static void main_periodic(int my_sig_num) {
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport,
|
||||
&estimator.tilt,&estimator.tilt_dot,
|
||||
&estimator.elevation,&estimator.elevation_dot,
|
||||
&estimator.azimuth,&estimator.azimuth_dot);});
|
||||
|
||||
RunOnceEvery(25, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
|
||||
&controller.cmd_pitch,&controller.cmd_thrust,
|
||||
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
|
||||
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
|
||||
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
|
||||
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
|
||||
&controller.azimuth_sp);});
|
||||
|
||||
//file_logger_periodic();
|
||||
|
||||
|
||||
/* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
|
||||
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
|
||||
|
||||
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
|
||||
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
|
||||
*/
|
||||
|
||||
/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
|
||||
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
|
||||
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
|
||||
|
||||
RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
|
||||
&estimator.tilt, &estimator.elevation, &estimator.azimuth );});
|
||||
*/
|
||||
|
||||
/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
|
||||
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
|
||||
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
|
||||
|
||||
RunOnceEvery(33, gcs_com_periodic());
|
||||
|
||||
}
|
||||
|
||||
#if 0
|
||||
static void main_parse_cmd_line(int argc, char *argv[]) {
|
||||
|
||||
if (argc>1){
|
||||
controller.kp = atof(argv[1]);
|
||||
// printf("kp set to %f\n",kp);
|
||||
if (argc>2) {
|
||||
controller.kd = atof(argv[2]);
|
||||
// printf("kd set to %f\n",kd);
|
||||
} else {
|
||||
controller.kd=1.0;
|
||||
// printf("using default value of kd %f\n",kd);
|
||||
}
|
||||
} else {
|
||||
controller.kp = 0.05;
|
||||
// printf("using default value of kp %f\n",kp);
|
||||
}
|
||||
/*
|
||||
if (argc>1){
|
||||
printf("args not currently supported\n");
|
||||
}*/
|
||||
}
|
||||
#endif
|
||||
|
||||
static void main_exit(int sig) {
|
||||
printf("Initiating BETH shutdown...\n");
|
||||
|
||||
@@ -268,6 +258,7 @@ static void main_exit(int sig) {
|
||||
}
|
||||
printf("done\n");
|
||||
#endif
|
||||
|
||||
//If a logfile is being used, close it.
|
||||
//file_logger_exit()
|
||||
printf("Main Overo Application Exiting...\n");
|
||||
|
||||
@@ -4,6 +4,9 @@
|
||||
#include "std.h"
|
||||
#include "stdio.h"
|
||||
|
||||
#include "messages2.h"
|
||||
#include "overo_gcs_com.h"
|
||||
|
||||
struct OveroController controller;
|
||||
|
||||
void control_init(void) {
|
||||
@@ -34,7 +37,7 @@ void control_init(void) {
|
||||
|
||||
controller.one_over_J = 2.;
|
||||
controller.mass = 5.;
|
||||
controller.azim_gain = 0.05;
|
||||
controller.azim_gain = 0.005;
|
||||
|
||||
controller.omega_cl = RadOfDeg(600);
|
||||
controller.xi_cl = 1.;
|
||||
@@ -51,7 +54,16 @@ void control_init(void) {
|
||||
controller.armed = 0;
|
||||
}
|
||||
|
||||
void control_send_messages(void) {
|
||||
|
||||
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
|
||||
&controller.cmd_pitch,&controller.cmd_thrust,
|
||||
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
|
||||
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
|
||||
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
|
||||
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
|
||||
&controller.azimuth_sp);});
|
||||
}
|
||||
|
||||
void control_run(void) {
|
||||
|
||||
|
||||
@@ -55,6 +55,7 @@ struct OveroController {
|
||||
extern struct OveroController controller;
|
||||
|
||||
extern void control_init(void);
|
||||
extern void control_send_messages(void);
|
||||
extern void control_run(void);
|
||||
|
||||
#endif /* OVERO_CONTROLLER_H */
|
||||
|
||||
@@ -3,6 +3,9 @@
|
||||
#include "booz/booz_imu.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "messages2.h"
|
||||
#include "overo_gcs_com.h"
|
||||
|
||||
struct OveroEstimator estimator;
|
||||
|
||||
void estimator_init(void) {
|
||||
@@ -11,6 +14,14 @@ void estimator_init(void) {
|
||||
estimator.azimuth_lp_coeff = 0.5;
|
||||
}
|
||||
|
||||
void estimator_send_messages(void) {
|
||||
|
||||
RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport,
|
||||
&estimator.tilt,&estimator.tilt_dot,
|
||||
&estimator.elevation,&estimator.elevation_dot,
|
||||
&estimator.azimuth,&estimator.azimuth_dot);});
|
||||
}
|
||||
|
||||
//bench sensors z,y,x values passed in
|
||||
void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) {
|
||||
|
||||
|
||||
@@ -22,6 +22,7 @@ struct OveroEstimator {
|
||||
extern struct OveroEstimator estimator;
|
||||
|
||||
extern void estimator_init(void);
|
||||
extern void estimator_send_messages(void);
|
||||
extern void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure);
|
||||
|
||||
#endif /* OVERO_CONTROLLER_H */
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
#include "overo_sfb_controller.h"
|
||||
|
||||
#include "overo_estimator.h"
|
||||
#include "std.h"
|
||||
#include "stdio.h"
|
||||
|
||||
#include "messages2.h"
|
||||
#include "overo_gcs_com.h"
|
||||
|
||||
#define _CO (controller)
|
||||
|
||||
struct OveroController _CO;
|
||||
|
||||
void control_send_messages(void) {
|
||||
|
||||
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_SFB(gcs_com.udp_transport,
|
||||
&_CO.tilt_sp);});
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void control_init(void) {
|
||||
|
||||
_CO.tilt_sp = 0.;
|
||||
_CO.elevation_sp = RadOfDeg(10);
|
||||
_CO.azimuth_sp = 0.;
|
||||
|
||||
_CO.tilt_ref = 0.;
|
||||
_CO.elevation_ref = 0;
|
||||
_CO.azimuth_ref = 0.;
|
||||
|
||||
_CO.tilt_dot_ref = 0.;
|
||||
_CO.elevation_dot_ref = 0;
|
||||
_CO.azimuth_dot_ref = 0.;
|
||||
|
||||
_CO.cmd_pitch = 0.;
|
||||
_CO.cmd_thrust = 0.;
|
||||
|
||||
_CO.armed = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void control_run(void) {
|
||||
|
||||
static int foo=0;
|
||||
|
||||
/*
|
||||
* calculate errors
|
||||
*/
|
||||
|
||||
const float err_tilt = estimator.tilt - _CO.tilt_ref;
|
||||
const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;
|
||||
|
||||
const float err_elevation = estimator.elevation - _CO.elevation_ref;
|
||||
const float err_elevation_dot = estimator.elevation_dot - _CO.elevation_dot_ref;
|
||||
|
||||
const float err_azimuth = estimator.azimuth - _CO.azimuth_ref;
|
||||
const float err_azimuth_dot = estimator.azimuth_dot - _CO.azimuth_dot_ref;
|
||||
|
||||
/*
|
||||
* Compute state feedback
|
||||
*/
|
||||
|
||||
_CO.cmd_pitch = 1;
|
||||
estimator.azimuth
|
||||
* ( _CO.o_tilt * _CO.o_tilt * _CO.o_azim * _CO.o_azim * cos(estimator.tilt) )
|
||||
/ ( _CO.b * _CO.a * _CO.u_t_ref) +
|
||||
estimator.elevation
|
||||
* ( _CO.o_tilt * _CO.o_tilt * _CO.o_elev * _CO.o_elev * sin(estimator.tilt) )
|
||||
/ ( _CO.b * _CO.a * _CO.u_t_ref) -
|
||||
estimator.tilt
|
||||
* ( _CO.o_tilt * _CO.o_tilt ) / ( _CO.b ) +
|
||||
estimator.azimuth_dot
|
||||
* ( _CO.o_tilt * _CO.o_tilt * 2 * _CO.z_azim * _CO.o_azim * cos(estimator.tilt) )
|
||||
/ ( _CO.b * _CO.a * _CO.u_t_ref) +
|
||||
estimator.elevation_dot
|
||||
* ( _CO.o_tilt * _CO.o_tilt * 2 * _CO.z_elev * _CO.o_elev * sin(estimator.tilt) )
|
||||
/ ( _CO.b * _CO.a * _CO.u_t_ref) -
|
||||
estimator.tilt_dot
|
||||
* ( 2 * _CO.o_tilt * _CO.z_tilt )
|
||||
/ ( _CO.b ) ;
|
||||
|
||||
_CO.cmd_thrust =
|
||||
estimator.azimuth * _CO.o_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a +
|
||||
estimator.elevation * _CO.o_elev * _CO.o_elev * cos(estimator.tilt) / _CO.a +
|
||||
estimator.azimuth_dot * 2 * _CO.z_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a -
|
||||
estimator.elevation_dot * 2 * _CO.z_elev * _CO.o_elev * cos(estimator.tilt) / _CO.a ;
|
||||
|
||||
_CO.cmd_thrust = _CO.cmd_thrust*(1/cos(estimator.elevation));
|
||||
|
||||
//if (_CO.cmd_thrust<0.) _CO.cmd_thrust = 0;
|
||||
Bound(_CO.cmd_thrust,0,100);
|
||||
Bound(_CO.cmd_pitch,-100,100);
|
||||
|
||||
if (!(foo%100)) {
|
||||
//printf("%f %f %f\n",_CO.tilt_ref,_CO.tilt_dot_ref,_CO.tilt_ddot_ref);
|
||||
}
|
||||
foo++;
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
#ifndef OVERO_CONTROLLER_H
|
||||
#define OVERO_CONTROLLER_H
|
||||
|
||||
struct OveroController {
|
||||
|
||||
float tilt_sp;
|
||||
float elevation_sp;
|
||||
float azimuth_sp;
|
||||
|
||||
float tilt_ref;
|
||||
float elevation_ref;
|
||||
float azimuth_ref;
|
||||
|
||||
float tilt_dot_ref;
|
||||
float elevation_dot_ref;
|
||||
float azimuth_dot_ref;
|
||||
|
||||
/*omegas - natural frequencies*/
|
||||
float o_tilt;
|
||||
float o_elev;
|
||||
float o_azim;
|
||||
|
||||
/*zetas - damping ratios*/
|
||||
float z_tilt;
|
||||
float z_elev;
|
||||
float z_azim;
|
||||
|
||||
/*constants*/
|
||||
|
||||
float a; // C_t0 / M
|
||||
float b; // l * C_t0 / J
|
||||
|
||||
float u_t_ref;
|
||||
|
||||
float cmd_pitch;
|
||||
float cmd_thrust;
|
||||
|
||||
int armed;
|
||||
};
|
||||
|
||||
|
||||
extern struct OveroController controller;
|
||||
|
||||
extern void control_init(void);
|
||||
extern void control_send_messages(void);
|
||||
extern void control_run(void);
|
||||
|
||||
#endif /* OVERO_CONTROLLER_H */
|
||||
@@ -0,0 +1,263 @@
|
||||
#include "overo_twist_controller.h"
|
||||
|
||||
#include "overo_estimator.h"
|
||||
#include "std.h"
|
||||
#include "stdio.h"
|
||||
#include "stdlib.h" //for abs()
|
||||
|
||||
#include "messages2.h"
|
||||
#include "overo_gcs_com.h"
|
||||
|
||||
struct OveroTwistController controller;
|
||||
|
||||
void control_send_messages(void) {
|
||||
|
||||
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
|
||||
&controller.cmd_pitch,&controller.cmd_thrust,
|
||||
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
|
||||
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
|
||||
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
|
||||
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
|
||||
&controller.azimuth_sp);});
|
||||
|
||||
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport,
|
||||
&controller.S[1],&controller.S_dot,&controller.U_twt[1],&controller.error);});
|
||||
}
|
||||
|
||||
|
||||
void control_init(void) {
|
||||
|
||||
printf("Twisting controller initializing\n");
|
||||
|
||||
controller.tilt_sp = 0.;
|
||||
controller.elevation_sp = RadOfDeg(10);
|
||||
controller.azimuth_sp = 0.;
|
||||
|
||||
controller.omega_tilt_ref = RadOfDeg(600);
|
||||
controller.omega_elevation_ref = RadOfDeg(120);
|
||||
controller.omega_azimuth_ref = RadOfDeg(60);
|
||||
controller.xi_ref = 1.;
|
||||
|
||||
controller.tilt_ref = estimator.tilt;
|
||||
controller.tilt_dot_ref = estimator.tilt_dot;
|
||||
controller.tilt_ddot_ref = 0.;
|
||||
|
||||
//controller.elevation_ref = estimator.elevation;
|
||||
controller.elevation_ref = RadOfDeg(-28);
|
||||
controller.elevation_dot_ref = estimator.elevation_dot;
|
||||
controller.elevation_ddot_ref = 0.;
|
||||
|
||||
controller.azimuth_ref = estimator.azimuth;
|
||||
controller.azimuth_dot_ref = 0.;
|
||||
controller.azimuth_ddot_ref = 0.;
|
||||
|
||||
controller.one_over_J = 2.;
|
||||
controller.mass = 5.;
|
||||
controller.azim_gain = 0.005;
|
||||
|
||||
controller.omega_cl = RadOfDeg(600);
|
||||
controller.xi_cl = 1.;
|
||||
|
||||
controller.cmd_pitch_ff = 0.;
|
||||
controller.cmd_pitch_fb = 0.;
|
||||
|
||||
controller.cmd_thrust_ff = 0.;
|
||||
controller.cmd_thrust_fb = 0.;
|
||||
|
||||
controller.cmd_pitch = 0.;
|
||||
controller.cmd_thrust = 0.;
|
||||
|
||||
controller.armed = 0;
|
||||
|
||||
/***** Coeficients twisting ****/
|
||||
controller.ulim = 1.0;
|
||||
controller.Vm = 0.1; //should this now be 1/512?
|
||||
controller.VM = 300.0;
|
||||
|
||||
controller.S[1] = 0.0;
|
||||
controller.S[0] = 0.0;
|
||||
|
||||
controller.U_twt[1] = 0.0;
|
||||
controller.U_twt[0] = 0.0;
|
||||
|
||||
controller.satval1 = 0.176;
|
||||
controller.satval2 = 1;
|
||||
|
||||
controller.c = 0.4;
|
||||
controller.error = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void control_run(void) {
|
||||
|
||||
/*
|
||||
* propagate reference
|
||||
*/
|
||||
const float dt_ctl = 1./512.;
|
||||
const float thrust_constant = 40.;
|
||||
|
||||
controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl;
|
||||
controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref * dt_ctl;
|
||||
controller.tilt_ddot_ref = -2*controller.omega_tilt_ref*controller.xi_ref*controller.tilt_dot_ref
|
||||
- controller.omega_tilt_ref*controller.omega_tilt_ref*(controller.tilt_ref - controller.tilt_sp);
|
||||
|
||||
controller.elevation_ref = controller.elevation_ref + controller.elevation_dot_ref * dt_ctl;
|
||||
controller.elevation_dot_ref = controller.elevation_dot_ref + controller.elevation_ddot_ref * dt_ctl;
|
||||
controller.elevation_ddot_ref = -2*controller.omega_elevation_ref*controller.xi_ref*controller.elevation_dot_ref
|
||||
- controller.omega_elevation_ref*controller.omega_elevation_ref*(controller.elevation_ref - controller.elevation_sp);
|
||||
|
||||
controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl;
|
||||
controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl;
|
||||
controller.azimuth_ddot_ref = -2*controller.omega_azimuth_ref*controller.xi_ref*controller.azimuth_dot_ref
|
||||
- controller.omega_azimuth_ref*controller.omega_azimuth_ref*(controller.azimuth_ref - controller.azimuth_sp);
|
||||
|
||||
static int foo=0;
|
||||
|
||||
/*
|
||||
* calculate errors
|
||||
*/
|
||||
|
||||
/* const float err_tilt = estimator.tilt - controller.tilt_ref;
|
||||
const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;*/
|
||||
|
||||
const float err_elevation = estimator.elevation - controller.elevation_ref;
|
||||
const float err_elevation_dot = estimator.elevation_dot - controller.elevation_dot_ref;
|
||||
|
||||
const float err_azimuth = estimator.azimuth - controller.azimuth_ref;
|
||||
const float err_azimuth_dot = estimator.azimuth_dot - controller.azimuth_dot_ref;
|
||||
|
||||
/*
|
||||
* Compute feedforward and feedback commands
|
||||
*/
|
||||
|
||||
controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref;
|
||||
|
||||
/* controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) +
|
||||
controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt);*/
|
||||
|
||||
controller.cmd_pitch_fb = get_U_twt();
|
||||
|
||||
controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref;
|
||||
controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) -
|
||||
controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
|
||||
|
||||
controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref;
|
||||
controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) +
|
||||
controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth);
|
||||
|
||||
controller.cmd_pitch = /*controller.cmd_pitch_ff*/ + controller.cmd_pitch_fb;
|
||||
|
||||
//controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb );
|
||||
|
||||
controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
|
||||
controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));
|
||||
|
||||
//if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
|
||||
Bound(controller.cmd_thrust,0,100);
|
||||
Bound(controller.cmd_pitch,-100,100);
|
||||
|
||||
if (!(foo%128)) {
|
||||
//printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot);
|
||||
//printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
|
||||
//printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
|
||||
printf("t: %f\n",controller.cmd_pitch_fb);
|
||||
}
|
||||
foo++;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*Fonction qui obtient la commande twisiting à appliquer chaque periode*/
|
||||
float get_U_twt()
|
||||
{
|
||||
|
||||
/**Definition des constantes du modèle**/
|
||||
const float Res = 0.4 ;
|
||||
const double Kphi = 0.0129;
|
||||
const double alpha = 3.2248e-7 ;
|
||||
const float cte = 60.0 ;
|
||||
const float Te = 1/512.;
|
||||
|
||||
/**Variables utilisés par la loi de commande**/
|
||||
static volatile float yd[2] = {0.0,0.0};
|
||||
static volatile float y[2] = {0.,0.};
|
||||
//static float emax = 0.035; // en rad, initialement 2
|
||||
|
||||
//Variables auxiliaires utilisés
|
||||
static volatile int aux_y = 0;
|
||||
|
||||
/**Variables pour l'algorithme**/
|
||||
float udot;
|
||||
float sens;
|
||||
|
||||
/**Acquisiton des donnes**/
|
||||
//Acquisition consigne
|
||||
yd[1] = controller.tilt_ref;
|
||||
//Acquisition mesure
|
||||
y[1] = estimator.tilt;
|
||||
|
||||
//On initialise au début angle courant=angle anterieur
|
||||
if (aux_y == 0){
|
||||
y[0] = y[1];
|
||||
aux_y = 1;
|
||||
}
|
||||
|
||||
/***************************/
|
||||
|
||||
/**Calcul Surface et derive Surface**/
|
||||
// S[1],y[1],yd[1] new value
|
||||
// S[0],y[0],yd[0] last value
|
||||
|
||||
//gain K=Te
|
||||
//controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ;
|
||||
controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ;
|
||||
//controller.S[1] = (float)( ( controller.c * (y[1]-yd[1]) ) + (estimator.tilt_dot - controller.tilt_dot_ref) );
|
||||
controller.S_dot = (controller.S[1] - controller.S[0]);
|
||||
/*************************************/
|
||||
|
||||
//On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure
|
||||
/* if ( abs(y[1] - yd[1]) < emax ) {
|
||||
U_twt[1] = U_twt[0];
|
||||
} else {*/
|
||||
/**Algorithme twisting**/
|
||||
if ( controller.S[1] < 0.0 ) sens = -1.0;
|
||||
else if ( controller.S[1] > 0.0 ) sens = 1.0;
|
||||
if ( abs(controller.U_twt[1]) < controller.ulim ) {
|
||||
if ( (controller.S[1] * controller.S_dot) > 0) {
|
||||
udot = -controller.VM * sens;
|
||||
}
|
||||
else {
|
||||
udot = -controller.Vm * sens;
|
||||
}
|
||||
}
|
||||
else {
|
||||
udot = -controller.U_twt[1];
|
||||
}
|
||||
|
||||
// Integration de u, qu'avec 2 valeurs, penser à faire plus
|
||||
// u[1] new , u[0] old
|
||||
controller.U_twt[1] = controller.U_twt[0] + (Te * udot);
|
||||
//}
|
||||
/**********************/
|
||||
|
||||
/**Saturation de l'integrateur**/
|
||||
|
||||
if ( (controller.S[1] > -controller.satval1) && (controller.S[1] < controller.satval1) ){
|
||||
Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
|
||||
}
|
||||
else {
|
||||
Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
|
||||
}
|
||||
/********************************/
|
||||
|
||||
/**Mises à jour**/
|
||||
controller.U_twt[0] = controller.U_twt[1];
|
||||
yd[0] = yd[1];
|
||||
y[0] = y[1];
|
||||
|
||||
controller.S[0] = controller.S[1];
|
||||
|
||||
return -80000. * (cte * Res * alpha/(2 * Kphi) ) * controller.U_twt[1];
|
||||
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
#ifndef OVERO_TWIST_CONTROLLER_H
|
||||
#define OVERO_TWIST_CONTROLLER_H
|
||||
|
||||
struct OveroTwistController {
|
||||
// float kp;
|
||||
// float kd;
|
||||
|
||||
float tilt_sp;
|
||||
float elevation_sp;
|
||||
float azimuth_sp;
|
||||
|
||||
/* modele de reference */
|
||||
float tilt_ref;
|
||||
float tilt_dot_ref;
|
||||
float tilt_ddot_ref;
|
||||
|
||||
float elevation_ref;
|
||||
float elevation_dot_ref;
|
||||
float elevation_ddot_ref;
|
||||
|
||||
float azimuth_ref;
|
||||
float azimuth_dot_ref;
|
||||
float azimuth_ddot_ref;
|
||||
|
||||
float omega_tilt_ref;
|
||||
float omega_elevation_ref;
|
||||
float omega_azimuth_ref;
|
||||
float xi_ref;
|
||||
|
||||
/* invert control law parameter */
|
||||
float one_over_J;
|
||||
float mass;
|
||||
|
||||
/* closed loop parameters */
|
||||
float omega_cl;
|
||||
float xi_cl;
|
||||
float azim_gain;
|
||||
|
||||
float cmd_pitch_ff;
|
||||
float cmd_pitch_fb;
|
||||
|
||||
float cmd_thrust_ff;
|
||||
float cmd_thrust_fb;
|
||||
|
||||
float cmd_azimuth_ff;
|
||||
float cmd_azimuth_fb;
|
||||
|
||||
float cmd_pitch;
|
||||
float cmd_thrust;
|
||||
|
||||
int armed;
|
||||
|
||||
/***Twisting stuff***/
|
||||
|
||||
float S[2];
|
||||
float S_dot;
|
||||
float U_twt[2];
|
||||
|
||||
/***** Coeficients twisting ****/
|
||||
float ulim;
|
||||
float Vm;
|
||||
float VM;
|
||||
|
||||
float satval1;
|
||||
float satval2;
|
||||
|
||||
float c;
|
||||
|
||||
float error;
|
||||
|
||||
};
|
||||
|
||||
|
||||
extern struct OveroTwistController controller;
|
||||
|
||||
extern void control_init(void);
|
||||
extern void control_send_messages(void);
|
||||
extern void control_run(void);
|
||||
float get_U_twt(void);
|
||||
|
||||
#endif /* OVERO_TWIST_CONTROLLER_H */
|
||||
Reference in New Issue
Block a user