[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5266] Adding elevation control.
|
From: |
Paul Cox |
|
Subject: |
[paparazzi-commits] [5266] Adding elevation control. |
|
Date: |
Fri, 06 Aug 2010 16:48:06 +0000 |
Revision: 5266
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5266
Author: paulcox
Date: 2010-08-06 16:48:05 +0000 (Fri, 06 Aug 2010)
Log Message:
-----------
Adding elevation control. tested on beth. need to address shortcomings of
estimator (no rotations taken into account) and controller (cos of elevation
angle not taken into account of thrust)
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/beth.xml
paparazzi3/trunk/conf/messages.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
Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-06 16:43:52 UTC
(rev 5265)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml 2010-08-06 16:48:05 UTC
(rev 5266)
@@ -29,8 +29,8 @@
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_SIGN" value="-1"/>
- <define name="GYRO_Q_SIGN" value="-1"/>
- <define name="GYRO_R_SIGN" value=" 1"/>
+ <define name="GYRO_Q_SIGN" value="1"/>
+ <define name="GYRO_R_SIGN" value="1"/>
<define name="GYRO_P_NEUTRAL" value="31288"/>
<define name="GYRO_Q_NEUTRAL" value="32593"/>
@@ -215,7 +215,7 @@
main_overo.ARCHDIR = omap
main_overo.CFLAGS = -I. -I$(SRC_FMS)
main_overo.srcs = $(SRC_BETH)/main_overo.c
-main_overo.CFLAGS += -DFMS_PERIODIC_FREQ=500
+main_overo.CFLAGS += -DFMS_PERIODIC_FREQ=512
main_overo.srcs += $(SRC_FMS)/fms_periodic.c
main_overo.srcs += $(SRC_FMS)/fms_serial_port.c
main_overo.LDFLAGS += -lrt
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-08-06 16:43:52 UTC (rev 5265)
+++ paparazzi3/trunk/conf/messages.xml 2010-08-06 16:48:05 UTC (rev 5266)
@@ -601,6 +601,31 @@
<field name="other" type="uint16"/>
</message>
+ <message name="BETH_ESTIMATOR" id="107">
+ <field name="tilt" type="float"/>
+ <field name="tilt_dot" type="float"/>
+ <field name="elevation" type="float"/>
+ <field name="elevation_dot" type="float"/>
+ <field name="azimuth" type="float"/>
+ <field name="azimuth_dot" type="float"/>
+ </message>
+
+ <message name="BETH_CONTROLLER" id="108">
+ <field name="pitch" type="float"/>
+ <field name="thrust" type="float"/>
+ <field name="pitch_ff" type="float"/>
+ <field name="pitch_fb" type="float"/>
+ <field name="thrust_ff" type="float"/>
+ <field name="thrust_fb" type="float"/>
+ <field name="tilt_ref" type="float"/>
+ <field name="tilt_dot_ref" type="float"/>
+ <field name="elevation_ref " type="float"/>
+ <field name="elevation_dot_ref" type="float"/>
+ </message>
+
+
+
+
<message name="DC_SHOT" id="110">
<field name="photo_nr" type="int16" unit=""></field>
<field name="utm_east" type="int32" unit="cm"></field>
Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-06 16:43:52 UTC
(rev 5265)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c 2010-08-06 16:48:05 UTC
(rev 5266)
@@ -63,7 +63,8 @@
int main(int argc, char *argv[]) {
(void) signal(SIGINT, main_exit);
-
+
+ //set IMU neutrals
RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL,
IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL,
IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL,
IMU_MAG_Z_NEUTRAL);
@@ -90,7 +91,7 @@
main_parse_cmd_line(argc, argv);
event_dispatch();
-
+ //should never occur!
printf("goodbye! (%d)\n",foo);
return 0;
@@ -99,6 +100,15 @@
static void main_periodic(int my_sig_num) {
+/* static int bar=0;
+ if (!(foo%2000)) {
+ if (bar) {
+ controller.tilt_sp = -0.4; bar=0;
+ }else{
+ controller.tilt_sp = 0.4; bar=1;
+ }
+ }
+*/
RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);});
@@ -108,15 +118,30 @@
RunOnceEvery(50,
{DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
&msg_in.bench_sensor.z,&foo);});
-
- estimator_run(msg_in.bench_sensor.z);
+
estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
+
+
+ controller.elevation_sp = ((foo/512.)%2) ? RadOfDeg(-15) : RadOfDeg(0);
+
control_run();
- // file_logger_periodic();
+ 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_ref,&controller.tilt_dot_ref
+
&controller.elevation_ref,&controller.elevation_dot_ref);});
- RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
+ //file_logger_periodic();
+
+
+/* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
//&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
@@ -124,13 +149,16 @@
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
//&msg_in.accel.x,&msg_in.accel.y,&msg_in.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,
+/* RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
//&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.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.accel.x,&msg_in.accel.y,&msg_in.accel.z
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
@@ -141,7 +169,7 @@
static void main_parse_cmd_line(int argc, char *argv[]) {
-
+#if 0
if (argc>1){
controller.kp = atof(argv[1]);
// printf("kp set to %f\n",kp);
@@ -156,7 +184,10 @@
controller.kp = 0.05;
// printf("using default value of kp %f\n",kp);
}
-
+#endif
+ if (argc>1){
+ printf("args not currently supported\n");
+ }
}
@@ -172,31 +203,23 @@
static int8_t adder = 1;
//uint8_t *fooptr;
- msg_out.thrust = 10;
+ //msg_out.thrust = 0;
+ msg_out.thrust = (int8_t)controller.cmd_thrust;
- if (!(foo%100)) {
- /*if (pitchval == 15 ) adder=-1;
- if (pitchval == -15 ) adder=1;
- pitchval = pitchval + adder;*/
- printf("cmd now %f\n",controller.cmd);
- }
- msg_out.pitch = (int8_t)controller.cmd;
- //msg_out.pitch = 1;
- //msg_out.cksum = msg_out.thrust + msg_out.pitch;
+ //TODO: change motor config to remove minus here.
+ msg_out.pitch = -(int8_t)controller.cmd_pitch;
+ //msg_out.pitch = 0;
spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);
- /*if (msg_in.bench_sensor.x == 0){
+ /* for debugging overo/stm spi link
+ if (msg_in.bench_sensor.x == 0){
fooptr = &msg_in;
for (int i=0; i<sizeof(msg_in); i++)
printf("%02x ", fooptr[i]);
printf("\n");
}*/
- /*if (!(foo%10)) {
- printf("%d %d %d %x\r\n",msg_in.bench_sensor.x,msg_in.bench_sensor.y,
- msg_in.bench_sensor.z,msg_in.bench_sensor.x);
- }*/
foo++;
booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-06
16:43:52 UTC (rev 5265)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c 2010-08-06
16:48:05 UTC (rev 5266)
@@ -2,31 +2,42 @@
#include "overo_estimator.h"
#include "std.h"
+#include "stdio.h"
struct OveroController controller;
void control_init(void) {
- controller.kp = 0.05;
- controller.kd = 0.01;
+// controller.kp = 0.05;
+// controller.kd = 0.01;
controller.tilt_sp = 0.;
-
- controller.omega_ref = RadOfDeg(200);
+ controller.elevation_sp = 0.;
+
+ controller.omega_tilt_ref = RadOfDeg(200);
+ controller.omega_elevation_ref = RadOfDeg(50);
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_dot_ref = estimator.elevation_dot;
+ controller.elevation_ddot_ref = 0.;
+
controller.one_over_J = 1.;
controller.omega_cl = RadOfDeg(500);
controller.xi_cl = 1.;
- controller.cmd_ff = 0.;
- controller.cmd_fb1 = 0.;
- controller.cmd_fb2 = 0.;
- controller.cmd = 0.;
+ 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.;
}
@@ -37,35 +48,44 @@
* propagate reference
*/
const float dt_ctl = 1./500.;
+ 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_ref*controller.xi_ref*controller.tilt_dot_ref
- - controller.omega_ref*controller.omega_ref*(controller.tilt_ref -
controller.tilt_sp);
+ 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);
static int foo=0;
-#if 0
- float track_err = estimator.tilt - controller.tilt_sp;
- float pcmd = controller.kp*track_err;
- float dcmd = controller.kd*estimator.tilt_dot;
- //controller.cmd = controller.kp*track_err +
controller.kd*estimator.tilt_dot;
- controller.cmd = pcmd + dcmd;
-#else
+ 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_tilt = estimator.tilt - controller.tilt_ref;
- const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;
- controller.cmd_ff = controller.one_over_J*controller.tilt_ddot_ref;
- controller.cmd_fb1 =
controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot);
- controller.cmd_fb2 =
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
+ controller.cmd_thrust_ff =
controller.one_over_J*controller.elevation_ddot_ref;
+ controller.cmd_thrust_fb =
-controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot)
-
+
controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_elevation);
- controller.cmd = controller.cmd_ff + controller.cmd_fb1+ controller.cmd_fb2;
- if (!(foo%100))
- //printf("ff:%f fb:%f %f (%f)\n",controller.cmd_ff, controller.cmd_fb1,
controller.cmd_fb2,estimator.tilt_dot);
- printf("%f %f
%f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
+ controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;
+ controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb
+ thrust_constant;
+ if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
+
+ if (!(foo%100)) {
+ //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);
+ }
foo++;
-#endif
+
}
Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h 2010-08-06
16:43:52 UTC (rev 5265)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h 2010-08-06
16:48:05 UTC (rev 5266)
@@ -2,16 +2,23 @@
#define OVERO_CONTROLLER_H
struct OveroController {
- float kp;
- float kd;
+// float kp;
+// float kd;
float tilt_sp;
+ float elevation_sp;
/* modele de reference */
float tilt_ref;
float tilt_dot_ref;
float tilt_ddot_ref;
- float omega_ref;
+
+ float elevation_ref;
+ float elevation_dot_ref;
+ float elevation_ddot_ref;
+
+ float omega_tilt_ref;
+ float omega_elevation_ref;
float xi_ref;
/* invert control law parameter */
@@ -21,10 +28,14 @@
float omega_cl;
float xi_cl;
- float cmd_ff;
- float cmd_fb1;
- float cmd_fb2;
- float cmd;
+ float cmd_pitch_ff;
+ float cmd_pitch_fb;
+
+ float cmd_thrust_ff;
+ float cmd_thrust_fb;
+
+ float cmd_pitch;
+ float cmd_thrust;
};
Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-06 16:43:52 UTC
(rev 5265)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-06 16:48:05 UTC
(rev 5266)
@@ -9,16 +9,25 @@
}
-void estimator_run(uint16_t tilt_measure) {
+void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t
azimuth_measure) {
const int32_t tilt_neutral = 2815;
const float tilt_scale = 1./580.;
- //const float tilt_scale = 1.;
+ const int32_t azimuth_neutral = 1500;
+ const float azimuth_scale = 1./580.;
+ const int32_t elevation_neutral = 1050;
+ const float elevation_scale = 1./580.;
- estimator.tilt = (tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
- //estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
+
+ estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
estimator.tilt_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.q);
+ //TODO: Add rotation compensation to remove tilt effect on elev/azi
+ estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) *
elevation_scale;
+ estimator.elevation_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.p);
+ estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) *
azimuth_scale;
+ estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r);
+
}
Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-06 16:43:52 UTC
(rev 5265)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-06 16:48:05 UTC
(rev 5266)
@@ -9,14 +9,15 @@
float elevation;
float tilt;
+ float azimuth_dot;
+ float elevation_dot;
float tilt_dot;
-
};
extern struct OveroEstimator estimator;
extern void estimator_init(void);
-extern void estimator_run(uint16_t tilt_measure);
+extern void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure,
uint16_t azimuth_measure);
#endif /* OVERO_CONTROLLER_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5266] Adding elevation control.,
Paul Cox <=