diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index 2d246b6a58..ca9e5212f7 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -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 + # # # diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml index ded14dbc30..5540784647 100644 --- a/conf/settings/settings_beth.xml +++ b/conf/settings/settings_beth.xml @@ -4,15 +4,15 @@ - + - + - + diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c index 97b33869da..79e4ee1ce6 100644 --- a/sw/airborne/beth/main_overo.c +++ b/sw/airborne/beth/main_overo.c @@ -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"); diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c index e6e68879a5..3e4548f0a6 100644 --- a/sw/airborne/beth/overo_controller.c +++ b/sw/airborne/beth/overo_controller.c @@ -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) { diff --git a/sw/airborne/beth/overo_controller.h b/sw/airborne/beth/overo_controller.h index e16ad37eac..dcf27e756d 100644 --- a/sw/airborne/beth/overo_controller.h +++ b/sw/airborne/beth/overo_controller.h @@ -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 */ diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c index daac9d1937..d248d00e3b 100644 --- a/sw/airborne/beth/overo_estimator.c +++ b/sw/airborne/beth/overo_estimator.c @@ -3,6 +3,9 @@ #include "booz/booz_imu.h" #include +#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) { diff --git a/sw/airborne/beth/overo_estimator.h b/sw/airborne/beth/overo_estimator.h index a5214ee0c9..5150083c7a 100644 --- a/sw/airborne/beth/overo_estimator.h +++ b/sw/airborne/beth/overo_estimator.h @@ -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 */ diff --git a/sw/airborne/beth/overo_sfb_controller.c b/sw/airborne/beth/overo_sfb_controller.c new file mode 100644 index 0000000000..d204fb84f4 --- /dev/null +++ b/sw/airborne/beth/overo_sfb_controller.c @@ -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++; + +} + diff --git a/sw/airborne/beth/overo_sfb_controller.h b/sw/airborne/beth/overo_sfb_controller.h new file mode 100644 index 0000000000..8f688d8df4 --- /dev/null +++ b/sw/airborne/beth/overo_sfb_controller.h @@ -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 */ diff --git a/sw/airborne/beth/overo_twist_controller.c b/sw/airborne/beth/overo_twist_controller.c new file mode 100644 index 0000000000..049126ae87 --- /dev/null +++ b/sw/airborne/beth/overo_twist_controller.c @@ -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]; + +} diff --git a/sw/airborne/beth/overo_twist_controller.h b/sw/airborne/beth/overo_twist_controller.h new file mode 100644 index 0000000000..abffe99e5f --- /dev/null +++ b/sw/airborne/beth/overo_twist_controller.h @@ -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 */