[paparazzi-commits] [5195] working PD control of tilt with crista.

[Paul Cox]

Revision: 5195
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5195
Author:   paulcox
Date:     2010-08-02 15:59:11 +0000 (Mon, 02 Aug 2010)
Log Message:
-----------
working PD control of tilt with crista. TODO: timeout/checksum overolink, 
update to new CAN code, datalink to overo, kalman filter

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-01 12:22:51 UTC 
(rev 5194)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-02 15:59:11 UTC 
(rev 5195)
@@ -201,7 +201,8 @@
 USER =
 #HOST = beth
 #HOST = overo
-HOST = auto7
+#HOST = auto7
+HOST= regis
 TARGET_DIR = ~
 SRC_FMS=fms
 

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-01 12:22:51 UTC 
(rev 5194)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-02 15:59:11 UTC 
(rev 5195)
@@ -32,6 +32,7 @@
 #include "fms_debug.h"
 #include "fms_spi_link.h"
 #include "fms_autopilot_msg.h"
+#include "booz/booz_imu.h"
 
 #include <event.h>
 
@@ -51,37 +52,101 @@
 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 @@
   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;

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-01 12:22:51 UTC 
(rev 5194)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-02 15:59:11 UTC 
(rev 5195)
@@ -71,6 +71,7 @@
 
 
 static inline void main_periodic( void ) {
+  int8_t pitch;
   booz_imu_periodic();
 
   OveroLinkPeriodic(main_on_overo_link_lost)
@@ -89,13 +90,17 @@
     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_event( void ) {
   BoozImuEvent(on_gyro_accel_event, on_mag_event);
   OveroLinkEvent(main_on_overo_msg_received);
@@ -117,14 +123,22 @@
   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_link_lost(void) {
   //actuators_set(FALSE);
   my_cnt = 0;
+/* didn't work: */
+//  overo_link_arch_prepare_next_transfert();
+//  overo_link.status = IDLE;  
+
 }