Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 15:09:25 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

working PD control of tilt with crista. TODO: timeout/checksum overolink, update to new CAN code, datalink to overo, kalman filter

Browse Source
This commit is contained in:
Paul Cox
2010-08-02 15:59:11 +00:00
parent 3eb641287e
commit 41406363f0
3 changed files with 103 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/beth.xml
+2 -1
View File
@@ -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
sw/airborne/beth/main_overo.c
+78 -11
View File
@@ -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 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;
sw/airborne/beth/main_stm32.c
+23 -5
View File
@@ -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;
}