[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5795] SFB working and some minor tweaking on twisti
|
From: |
Paul Cox |
|
Subject: |
[paparazzi-commits] [5795] SFB working and some minor tweaking on twisting which is also working. |
|
Date: |
Fri, 03 Sep 2010 11:58:07 +0000 |
Revision: 5795
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5795
Author: paulcox
Date: 2010-09-03 11:58:06 +0000 (Fri, 03 Sep 2010)
Log Message:
-----------
SFB working and some minor tweaking on twisting which is also working.
Modified Paths:
--------------
paparazzi3/trunk/conf/settings/settings_beth_sfb.xml
paparazzi3/trunk/conf/settings/settings_beth_twist.xml
paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
Added Paths:
-----------
paparazzi3/trunk/conf/settings/settings_beth_pid.xml
Added: paparazzi3/trunk/conf/settings/settings_beth_pid.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth_pid.xml
(rev 0)
+++ paparazzi3/trunk/conf/settings/settings_beth_pid.xml 2010-09-03
11:58:06 UTC (rev 5795)
@@ -0,0 +1,31 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+ <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.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>
+
+ <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>
+
+ <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="estimator.azimuth_lp_coeff" min="0.01" step="0.01"
max="1" module="beth/overo_estimator" shortname="azim_lp_coeff">
+ </dl_setting>
+
+ <dl_setting var="controller.armed" min="0" step="1" max="2" shortname
="mode"/>
+
+ </dl_settings>
+ </dl_settings>
+</settings>
Modified: paparazzi3/trunk/conf/settings/settings_beth_sfb.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth_sfb.xml 2010-09-03
01:56:44 UTC (rev 5794)
+++ paparazzi3/trunk/conf/settings/settings_beth_sfb.xml 2010-09-03
11:58:06 UTC (rev 5795)
@@ -4,17 +4,11 @@
<dl_settings>
<dl_settings NAME="Controller">
- <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.elevation_sp" min="-25" step="1" max="20"
module="beth/overo_sfb_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_twist_controller" shortname="tilt_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_sfb_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_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">
</dl_setting>
@@ -23,29 +17,15 @@
<dl_setting var="estimator.azimuth_lp_coeff" min="0.01" step="0.01"
max="1" module="beth/overo_estimator" shortname="azim_lp_coeff">
</dl_setting>
-
- <dl_setting var="controller.ulim" min="0.1" step="0.1" max="50"
module="beth/overo_twist_controller" shortname="ulim">
+
+ <dl_setting var="controller.a" min=".001" step=".01" max=".06"
module="beth/overo_sfb_controller">
</dl_setting>
- <dl_setting var="controller.Vm" min="0.001" step="0.01" max="200"
module="beth/overo_twist_controller" shortname="Vm">
+
+ <dl_setting var="controller.b" min=".01" step=".01" max=".54"
module="beth/overo_sfb_controller">
</dl_setting>
- <dl_setting var="controller.VM" min="100" step="10" max="600"
module="beth/overo_twist_controller" shortname="VM">
- </dl_setting>
- <dl_setting var="controller.satval1" min="0" step="0.1" max="100"
module="beth/overo_twist_controller" shortname="satval1">
- </dl_setting>
- <dl_setting var="controller.satval2" min="0.01" step="0.1" max="4"
module="beth/overo_twist_controller" shortname="satval2">
- </dl_setting>
- <dl_setting var="controller.c" min="0.001" step="0.01" max="10"
module="beth/overo_twist_controller" shortname="c">
- </dl_setting>
- <!--float ulim; 1
- float Vm; .1
- float VM; 300
- float satval1; 0.4
- float satval2; 1
- const float c;.04-->
+ <dl_setting var="controller.armed" min="0" step="1" max="2"
module="beth/overo_sfb_controller" shortname ="mode"/>
- <dl_setting var="controller.armed" min="0" step="1" max="2" shortname
="mode"/>
-
</dl_settings>
</dl_settings>
</settings>
Modified: paparazzi3/trunk/conf/settings/settings_beth_twist.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth_twist.xml 2010-09-03
01:56:44 UTC (rev 5794)
+++ paparazzi3/trunk/conf/settings/settings_beth_twist.xml 2010-09-03
11:58:06 UTC (rev 5795)
@@ -24,22 +24,22 @@
<dl_setting var="estimator.azimuth_lp_coeff" min="0.01" step="0.01"
max="1" module="beth/overo_estimator" shortname="azim_lp_coeff">
</dl_setting>
- <dl_setting var="controller.ulim" min="0.1" step="0.1" max="50"
module="beth/overo_twist_controller" shortname="ulim">
+ <dl_setting var="controller.ulim" min="0.1" step="0.1" max="2"
module="beth/overo_twist_controller" shortname="ulim">
</dl_setting>
- <dl_setting var="controller.Vm" min="0.001" step="0.01" max="200"
module="beth/overo_twist_controller" shortname="Vm">
+ <dl_setting var="controller.Vm" min="0.001" step="0.01" max="0.2"
module="beth/overo_twist_controller" shortname="Vm">
</dl_setting>
- <dl_setting var="controller.VM" min="100" step="10" max="600"
module="beth/overo_twist_controller" shortname="VM">
+ <dl_setting var="controller.VM" min="100" step="10" max="700"
module="beth/overo_twist_controller" shortname="VM">
</dl_setting>
- <dl_setting var="controller.satval1" min="0" step="0.1" max="100"
module="beth/overo_twist_controller" shortname="satval1">
+ <dl_setting var="controller.satval1" min=".01" step="0.05" max="0.174"
module="beth/overo_twist_controller" shortname="satval1">
</dl_setting>
- <dl_setting var="controller.satval2" min="0.01" step="0.1" max="4"
module="beth/overo_twist_controller" shortname="satval2">
+ <dl_setting var="controller.satval2" min="0.01" step="0.01" max=".282"
module="beth/overo_twist_controller" shortname="satval2">
</dl_setting>
- <dl_setting var="controller.c" min="0.001" step="0.01" max="10"
module="beth/overo_twist_controller" shortname="c">
+ <dl_setting var="controller.c" min="0.001" step="0.1" max="9.5"
module="beth/overo_twist_controller" shortname="c">
</dl_setting>
- <!--float ulim; 1
- float Vm; .1
- float VM; 300
+ <!--float ulim; 1
+ float Vm; .1
+ float VM; 300
float satval1; 0.4
float satval2; 1
const float c;.04-->
Modified: paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c 2010-09-03
01:56:44 UTC (rev 5794)
+++ paparazzi3/trunk/sw/airborne/beth/overo_sfb_controller.c 2010-09-03
11:58:06 UTC (rev 5795)
@@ -3,6 +3,7 @@
#include "overo_estimator.h"
#include "std.h"
#include "stdio.h"
+#include "stdlib.h"
#include "messages2.h"
#include "overo_gcs_com.h"
@@ -19,8 +20,6 @@
}
-
-
void control_init(void) {
_CO.tilt_sp = 0.;
@@ -37,7 +36,21 @@
_CO.cmd_pitch = 0.;
_CO.cmd_thrust = 0.;
+
+ _CO.a = 0.03;
+ _CO.b = 0.27;
+ _CO.u_t_ref = 40;
+
+ /*omegas - natural frequencies*/
+ _CO.o_tilt = RadOfDeg(100);
+ _CO.o_elev = RadOfDeg(100);
+ _CO.o_azim = RadOfDeg(100);
+ /*zetas - damping ratios*/
+ _CO.z_tilt = 1;
+ _CO.z_elev = 1;
+ _CO.z_azim = 1;
+
_CO.armed = 0;
}
@@ -64,30 +77,30 @@
* Compute state feedback
*/
- _CO.cmd_pitch = 1;
- estimator.azimuth
+ _CO.cmd_pitch = -1*( -1*
+ err_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
+ err_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
+ err_tilt
+ * ( _CO.o_tilt * _CO.o_tilt ) / ( _CO.b ) -//+
+ err_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
+ err_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
+ err_tilt_dot
* ( 2 * _CO.o_tilt * _CO.z_tilt )
- / ( _CO.b ) ;
+ / ( _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 ;
+ err_azimuth * _CO.o_azim * _CO.o_azim * sin(estimator.tilt) /
_CO.a -
+ err_elevation * _CO.o_elev * _CO.o_elev * cos(estimator.tilt) /
_CO.a +
+ err_azimuth_dot * 2 * _CO.z_azim * _CO.o_azim * sin(estimator.tilt) /
_CO.a -
+ err_elevation_dot * 2 * _CO.z_elev * _CO.o_elev * cos(estimator.tilt) /
_CO.a ;
_CO.cmd_thrust = _CO.cmd_thrust*(1/cos(estimator.elevation));
@@ -96,7 +109,7 @@
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);
+ printf("P:%f T:%f \n",_CO.cmd_pitch,_CO.cmd_thrust);
}
foo++;
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5795] SFB working and some minor tweaking on twisting which is also working.,
Paul Cox <=