diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml
index 9ef63b4717..31bda17594 100644
--- a/conf/settings/settings_beth.xml
+++ b/conf/settings/settings_beth.xml
@@ -6,10 +6,16 @@
-
+
-
+
+
+
+
+
+
+
diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c
index 64d903e530..a28fa13aaf 100644
--- a/sw/airborne/beth/main_overo.c
+++ b/sw/airborne/beth/main_overo.c
@@ -126,7 +126,8 @@ static void main_periodic(int my_sig_num) {
&msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs,
&msg_in.payload.msg_up.thrust_out,&msg_in.payload.msg_up.pitch_out);});
- estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,msg_in.payload.msg_up.bench_sensor.x);
+ estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,
+ msg_in.payload.msg_up.bench_sensor.x);
if ( msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is lost! %d %d\n",
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index f0e214b240..fda95175f2 100644
--- a/sw/airborne/beth/overo_controller.c
+++ b/sw/airborne/beth/overo_controller.c
@@ -26,8 +26,13 @@ void control_init(void) {
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 = 1.;
controller.omega_cl = RadOfDeg(600);
controller.xi_cl = 1.;
@@ -66,21 +71,35 @@ void control_run(void) {
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;
- 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);
+ const float err_azimuth = estimator.azimuth - controller.azimuth_ref;
+ const float err_azimuth_dot = estimator.azimuth_dot - controller.azimuth_dot_ref;
- 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);
+ /*
+ * Compute feedforward and feedback commands
+ */
- controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;
+ 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_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_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.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));
diff --git a/sw/airborne/beth/overo_controller.h b/sw/airborne/beth/overo_controller.h
index 14d730b785..d1e72307d2 100644
--- a/sw/airborne/beth/overo_controller.h
+++ b/sw/airborne/beth/overo_controller.h
@@ -17,6 +17,10 @@ struct OveroController {
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 xi_ref;
@@ -28,6 +32,7 @@ struct OveroController {
/* closed loop parameters */
float omega_cl;
float xi_cl;
+ float azim_gain;
float cmd_pitch_ff;
float cmd_pitch_fb;
@@ -35,6 +40,8 @@ struct OveroController {
float cmd_thrust_ff;
float cmd_thrust_fb;
+ float cmd_azimuth_fb;
+
float cmd_pitch;
float cmd_thrust;
diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c
index d5e197a5bb..ab87e7a902 100644
--- a/sw/airborne/beth/overo_estimator.c
+++ b/sw/airborne/beth/overo_estimator.c
@@ -6,15 +6,16 @@
struct OveroEstimator estimator;
void estimator_init(void) {
- estimator.lp_coeff = 0.9;
+ estimator.tilt_lp_coeff = 0.9;
+ estimator.elevation_lp_coeff = 0.9;
}
-//zyx
+//bench sensors z,y,x values passed in
void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) {
const int32_t tilt_neutral = 1970;
const float tilt_scale = 1./580.;
- const int32_t azimuth_neutral = 1500;
+ const int32_t azimuth_neutral = 2875;
const float azimuth_scale = 1./580.;
const int32_t elevation_neutral = 670;
const float elevation_scale = 1./580.;
@@ -22,16 +23,25 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a
estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
Bound(estimator.tilt,-89,89);
- //TODO: lp filter tilt_dot
- estimator.tilt_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.q);
+ //low pass filter gyro values
+ estimator.tilt_dot = estimator.tilt_dot +
+ estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot);
+ /* Second order filter to be tested
+ estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) +
+ estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(booz_imu.gyro.q) -
+ estimator.tilt_dot_old * (1 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 +
+ estimator.tilt_lp_coeff1*estimator.tilt_lp_coeff2);
+ */
estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) * elevation_scale;
Bound(estimator.elevation,-45,45);
//rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes
- float rotated_elev_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p)*cos(estimator.tilt)+RATE_FLOAT_OF_BFP(booz_imu.gyro.r)*sin(estimator.tilt);
+ float rotated_elev_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p) * cos(estimator.tilt) +
+ RATE_FLOAT_OF_BFP(booz_imu.gyro.r) * sin(estimator.tilt);
//low pass filter -- should probably increase order and maybe move filtering to measured values.
- estimator.elevation_dot = estimator.elevation_dot +estimator.lp_coeff*(rotated_elev_dot-estimator.elevation_dot);
+ estimator.elevation_dot = estimator.elevation_dot +
+ estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot);
estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * azimuth_scale;
estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r);
diff --git a/sw/airborne/beth/overo_estimator.h b/sw/airborne/beth/overo_estimator.h
index b9bcf7fd7f..94056c048e 100644
--- a/sw/airborne/beth/overo_estimator.h
+++ b/sw/airborne/beth/overo_estimator.h
@@ -13,7 +13,8 @@ struct OveroEstimator {
float elevation_dot;
float tilt_dot;
- float lp_coeff;
+ float tilt_lp_coeff;
+ float elevation_lp_coeff;
};