[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5710] Adding twisting and state feedback controller
|
From: |
Paul Cox |
|
Subject: |
[paparazzi-commits] [5710] Adding twisting and state feedback controllers. |
|
Date: |
Thu, 26 Aug 2010 15:40:17 +0000 |
Revision: 5710
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5710
Author: paulcox
Date: 2010-08-26 15:40:02 +0000 (Thu, 26 Aug 2010)
Log Message:
-----------
Adding twisting and state feedback controllers. Twisting is done on tilt axis
only and works mostly. sfb is just a start.
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/beth.xml
paparazzi3/trunk/conf/settings/settings_beth.xml
paparazzi3/trunk/sw/airborne/beth/main_overo.c
paparazzi3/trunk/sw/airborne/beth/overo_controller.c
paparazzi3/trunk/sw/airborne/beth/overo_controller.h
paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.h
paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c
paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h
Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-26 15:16:52 UTC
(rev 5709)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-26 15:40:02 UTC
(rev 5710)
@@ -238,12 +238,73 @@
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
+
+#
+#
+#
include $(PAPARAZZI_SRC)/conf/autopilot/lisa_test_progs.makefile
</makefile>
Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml 2010-08-26 15:16:52 UTC
(rev 5709)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml 2010-08-26 15:40:02 UTC
(rev 5710)
@@ -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">
Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-26 15:16:52 UTC
(rev 5709)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-26 15:40:02 UTC
(rev 5710)
@@ -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 @@
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 @@
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 @@
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 @@
}
printf("done\n");
#endif
+
//If a logfile is being used, close it.
//file_logger_exit()
printf("Main Overo Application Exiting...\n");
Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-26
15:16:52 UTC (rev 5709)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-26
15:40:02 UTC (rev 5710)
@@ -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 @@
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 @@
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) {
Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h 2010-08-26
15:16:52 UTC (rev 5709)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h 2010-08-26
15:40:02 UTC (rev 5710)
@@ -55,6 +55,7 @@
extern struct OveroController controller;
extern void control_init(void);
+extern void control_send_messages(void);
extern void control_run(void);
#endif /* OVERO_CONTROLLER_H */
Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-26 15:16:52 UTC
(rev 5709)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-26 15:40:02 UTC
(rev 5710)
@@ -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 @@
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) {
Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-26 15:16:52 UTC
(rev 5709)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-26 15:40:02 UTC
(rev 5710)
@@ -22,6 +22,7 @@
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 */
Added: paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c 2010-08-26
15:40:02 UTC (rev 5710)
@@ -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++;
+
+}
+
Added: paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.h 2010-08-26
15:40:02 UTC (rev 5710)
@@ -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 */
Added: paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.c 2010-08-26
15:40:02 UTC (rev 5710)
@@ -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];
+
+}
Added: paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_twist_controller.h 2010-08-26
15:40:02 UTC (rev 5710)
@@ -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 */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5710] Adding twisting and state feedback controllers.,
Paul Cox <=