From 41406363f09bb02804deeb8b75f8207cf7cdf0af Mon Sep 17 00:00:00 2001 From: Paul Cox Date: Mon, 2 Aug 2010 15:59:11 +0000 Subject: [PATCH] working PD control of tilt with crista. TODO: timeout/checksum overolink, update to new CAN code, datalink to overo, kalman filter --- conf/airframes/Poine/beth.xml | 3 +- sw/airborne/beth/main_overo.c | 89 ++++++++++++++++++++++++++++++----- sw/airborne/beth/main_stm32.c | 28 +++++++++-- 3 files changed, 103 insertions(+), 17 deletions(-) diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index 76b88de616..aee00e4247 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -201,7 +201,8 @@ main_coders.srcs += $(SRC_BETH)/bench_sensors_can.c USER = #HOST = beth #HOST = overo -HOST = auto7 +#HOST = auto7 +HOST= regis TARGET_DIR = ~ SRC_FMS=fms diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c index ff4434641e..d0b7037e3f 100644 --- a/sw/airborne/beth/main_overo.c +++ b/sw/airborne/beth/main_overo.c @@ -32,6 +32,7 @@ #include "fms_debug.h" #include "fms_spi_link.h" #include "fms_autopilot_msg.h" +#include "booz/booz_imu.h" #include @@ -51,37 +52,101 @@ static struct FmsNetwork* network; static struct AutopilotMessageBethUp msg_in; static struct AutopilotMessageBethDown msg_out; static void send_message(void); +static void PID(void); + +struct BoozImu booz_imu; +struct BoozImuFloat booz_imu_float; uint16_t az,elev,tilt; static uint32_t foo = 0; +//static int32_t p,q,r,x,y,z; static void main_periodic(int my_sig_num) { RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + PID(); send_message(); RunOnceEvery(10, {DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y, &msg_in.bench_sensor.z,&foo);}); + booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF); + booz_imu.gyro_unscaled.q = (msg_in.gyro.q&0xFFFF); + booz_imu.gyro_unscaled.r = (msg_in.gyro.r&0xFFFF); + booz_imu.accel_unscaled.x = (msg_in.accel.x&0xFFFF); + booz_imu.accel_unscaled.y = (msg_in.accel.y&0xFFFF); + booz_imu.accel_unscaled.z = (msg_in.accel.z&0xFFFF); + + BoozImuScaleGyro(); + + RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, + //&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);}); + + + RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, + //&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(10, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, - &msg_in.gyro.p, - &msg_in.gyro.q, - &msg_in.gyro.r);}); + //&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(10, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, - &msg_in.accel.x, - &msg_in.accel.y, - &msg_in.accel.z);}); + //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z + &booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);}); RunOnceEvery(10, {UdpTransportPeriodic();}); } +static int8_t pitchval = 0; +static float kp, ki, kd; +int8_t presp,dresp; +static uint16_t tilt_sp=2600; +static float piderror,piderrorold; + +static void PID(){ + piderror = tilt_sp-msg_in.bench_sensor.z; + + presp = (int8_t)(kp * piderror); + //dresp = (int8_t)(kd * (piderror - piderrorold) ); + dresp = (int8_t)(kd * booz_imu.gyro.q); + + pitchval = presp + dresp; + + piderrorold = piderror; + + if (!(foo%100)) { + printf("%d %d\n",presp,dresp); + } +} int main(int argc, char *argv[]) { + + if (argc>1){ + kp = atof(argv[1]); + printf("kp set to %f\n",kp); + if (argc>2) { + kd = atof(argv[2]); + printf("kd set to %f\n",kd); + } else { + kd=1.0; + printf("using default value of kd %f\n",kd); + } + } else { + kp = 0.05; + printf("using default value of kp %f\n",kp); + } + ki=0.0; + + 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); + if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; @@ -107,18 +172,20 @@ int main(int argc, char *argv[]) { return 0; } -static int8_t pitchval = 0; + + +//static int8_t pitchval = 0; static int8_t adder = 1; static void send_message() { //uint8_t *fooptr; - msg_out.thrust = 7; + msg_out.thrust = 10; if (!(foo%100)) { - if (pitchval == 10 ) adder=-1; - if (pitchval == -10 ) adder=1; - pitchval = pitchval + adder; + /*if (pitchval == 15 ) adder=-1; + if (pitchval == -15 ) adder=1; + pitchval = pitchval + adder;*/ printf("pitchval now %d\n",pitchval); } msg_out.pitch = pitchval; diff --git a/sw/airborne/beth/main_stm32.c b/sw/airborne/beth/main_stm32.c index da5010289e..117a5ff65c 100644 --- a/sw/airborne/beth/main_stm32.c +++ b/sw/airborne/beth/main_stm32.c @@ -71,6 +71,7 @@ static inline void main_init( void ) { static inline void main_periodic( void ) { + int8_t pitch; booz_imu_periodic(); OveroLinkPeriodic(main_on_overo_link_lost) @@ -89,13 +90,17 @@ static inline void main_periodic( void ) { always ongoing, and new data generates a flag by the IST. */ read_bench_sensors(); - booz2_commands[COMMAND_PITCH] = (int8_t)((0xFF) & overo_link.down.msg.pitch); + pitch = (int8_t)((0xFF) & overo_link.down.msg.pitch); + if (pitch > 10) pitch = 10; else + if (pitch < -10) pitch = -10; + + booz2_commands[COMMAND_PITCH] = pitch; booz2_commands[COMMAND_ROLL] = 0; booz2_commands[COMMAND_YAW] = 0; - if ( overo_link.down.msg.thrust < 6) { + if ( overo_link.down.msg.thrust < 10) { booz2_commands[COMMAND_THRUST] = overo_link.down.msg.thrust; } else { - booz2_commands[COMMAND_THRUST] = 5; + booz2_commands[COMMAND_THRUST] = 10; } if (my_cnt == 0) { actuators_set(FALSE); @@ -104,6 +109,7 @@ static inline void main_periodic( void ) { } } + static inline void main_event( void ) { BoozImuEvent(on_gyro_accel_event, on_mag_event); OveroLinkEvent(main_on_overo_msg_received); @@ -117,13 +123,21 @@ static inline void main_on_overo_msg_received(void) { overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2; overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3; - overo_link.up.msg.accel.x = booz_imu.accel.x; +/* overo_link.up.msg.accel.x = booz_imu.accel.x; overo_link.up.msg.accel.y = booz_imu.accel.y; overo_link.up.msg.accel.z = booz_imu.accel.z; overo_link.up.msg.gyro.p = booz_imu.gyro.p; overo_link.up.msg.gyro.q = booz_imu.gyro.q; - overo_link.up.msg.gyro.r = booz_imu.gyro.r; + overo_link.up.msg.gyro.r = booz_imu.gyro.r;*/ + + overo_link.up.msg.accel.x = booz_imu.accel_unscaled.x; + overo_link.up.msg.accel.y = booz_imu.accel_unscaled.y; + overo_link.up.msg.accel.z = booz_imu.accel_unscaled.z; + + overo_link.up.msg.gyro.p = booz_imu.gyro_unscaled.p; + overo_link.up.msg.gyro.q = booz_imu.gyro_unscaled.q; + overo_link.up.msg.gyro.r = booz_imu.gyro_unscaled.r; my_cnt=1; //actuators_set(TRUE); @@ -132,6 +146,10 @@ static inline void main_on_overo_msg_received(void) { static inline void main_on_overo_link_lost(void) { //actuators_set(FALSE); my_cnt = 0; +/* didn't work: */ +// overo_link_arch_prepare_next_transfert(); +// overo_link.status = IDLE; + }