mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
added feeback control of azimuth. A few other minor fixups.
This commit is contained in:
Paul Cox
@@ -6,10 +6,16 @@
|
||||
|
||||
<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.tilt_sp" min="-15" step="0.5" max="15" module="beth/overo_controller" shortname="tilt_sp" unit="rad" alt_unit="deg" alt_unit_coef="57.29578">
|
||||
<dl_setting var="controller.azimuth_ref" 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>
|
||||
|
||||
<dl_setting var="estimator.lp_coeff" min="0.01" step="0.01" max="1" module="beth/overo_estimator" shortname="elev_lp_coeff">
|
||||
<dl_setting var="controller.azim_gain" min="0.5" step=".05" max="5" module="beth/overo_estimator" shortname="azim_gain">
|
||||
</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">
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting var="estimator.tilt_lp_coeff" min="0.01" step="0.01" max="1" module="beth/overo_estimator" shortname="tilt_lp_coeff">
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting var="controller.armed" min="0" step="1" max="2" shortname ="mode"/>
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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));
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -13,7 +13,8 @@ struct OveroEstimator {
|
||||
float elevation_dot;
|
||||
float tilt_dot;
|
||||
|
||||
float lp_coeff;
|
||||
float tilt_lp_coeff;
|
||||
float elevation_lp_coeff;
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user