Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-02-21 12:02:40 +00:00
parent 73165b9998
commit 930a4965f9
15 changed files with 284 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+4 -4
View File
@@ -19,10 +19,10 @@
<let var="p" value="trim_p + @P"/>
<let var="q" value="trim_q + @Q"/>
<let var="r" value="trim_r + @R"/>
<set servo="MOTOR_FRONT" value="$throttle + $q - $r"/>
<set servo="MOTOR_BACK" value="$throttle - $q - $r"/>
<set servo="MOTOR_RIGHT" value="$throttle - $p + $r"/>
<set servo="MOTOR_LEFT" value="$throttle + $p + $r"/>
<set servo="MOTOR_FRONT" value="$throttle + $q + $r"/>
<set servo="MOTOR_BACK" value="$throttle - $q + $r"/>
<set servo="MOTOR_RIGHT" value="$throttle - $p - $r"/>
<set servo="MOTOR_LEFT" value="$throttle + $p - $r"/>
</command_laws>
<rc_commands>
conf/autopilot/conf_booz.makefile
+5 -4
View File
@@ -58,8 +58,9 @@ flt.srcs += scp1000.c $(SRC_ARCH)/scp1000_hw.c
flt.srcs += booz_still_detection.c
flt.CFLAGS += -DFLOAT_T=float
#flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT
#flt.srcs += ahrs_multitilt.c booz_ahrs.c
flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT
flt.srcs += ahrs_multitilt.c booz_ahrs.c
#flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_QUATERNION
#flt.srcs += ahrs_quat_fast_ekf.c booz_ahrs.c
@@ -67,8 +68,8 @@ flt.CFLAGS += -DFLOAT_T=float
#flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_EULER
#flt.srcs += ahrs_euler_fast_ekf.c booz_ahrs.c
flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_COMP_FILTER
flt.srcs += ahrs_comp_filter.c booz_ahrs.c
#flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_COMP_FILTER
#flt.srcs += ahrs_comp_filter.c booz_ahrs.c
flt.srcs += booz_inter_mcu.c
flt.srcs += booz_link_mcu.c $(SRC_ARCH)/booz_link_mcu_hw.c
conf/messages.xml
+14 -12
View File
@@ -454,21 +454,23 @@
</message>
<message name="BOOZ_RATE_LOOP" ID="233">
<field name="est_p" type="float" unit="rad/s"/>
<field name="sp_p" type="float" unit="rad/s"/>
<field name="est_q" type="float" unit="rad/s"/>
<field name="sp_q" type="float" unit="rad/s"/>
<field name="est_r" type="float" unit="rad/s"/>
<field name="sp_r" type="float" unit="rad/s"/>
<field name="p_est" type="float" unit="rad/s"/>
<field name="p_sp" type="float" unit="rad/s"/>
<field name="q_est" type="float" unit="rad/s"/>
<field name="q_sp" type="float" unit="rad/s"/>
<field name="r_est" type="float" unit="rad/s"/>
<field name="r_sp" type="float" unit="rad/s"/>
<field name="power_sp" type="float" unit="0-1"/>
</message>
<message name="BOOZ_ATT_LOOP" ID="234">
<field name="est_phi" type="float" unit="rad"/>
<field name="sp_phi" type="float" unit="rad"/>
<field name="est_theta" type="float" unit="rad"/>
<field name="sp_theta" type="float" unit="rad"/>
<field name="est_psi" type="float" unit="rad"/>
<field name="sp_psi" type="float" unit="rad"/>
<field name="phi_est" type="float" unit="rad"/>
<field name="phi_sp" type="float" unit="rad"/>
<field name="theta_est" type="float" unit="rad"/>
<field name="theta_sp" type="float" unit="rad"/>
<field name="psi_est" type="float" unit="rad"/>
<field name="psi_sp" type="float" unit="rad"/>
<field name="power_sp" type="float" unit="0-1"/>
</message>
<message name="BOOZ_VERT_LOOP" ID="235">
conf/telemetry/booz.xml
+8 -8
View File
@@ -12,10 +12,9 @@
<message name="ACTUATORS" period="0.5"/>
<message name="BOOZ_HOV_LOOP" period="0.05"/>
<message name="BOOZ_VERT_LOOP" period="0.05"/>
<message name="BOOZ_ATT_LOOP" period="0.05"/>
<message name="BOOZ_RATE_LOOP" period="0.05"/>
<message name="BOOZ_CMDS" period="0.05"/>
<message name="BOOZ_UF_RATES" period="0.05"/>
<message name="BOOZ_CONTROL" period="0.025"/>
<message name="BOOZ_CMDS" period="0.025"/>
<!-- <message name="BOOZ_UF_RATES" period="0.05"/> -->
<message name="DL_VALUE" period="1."/>
</mode>
<mode name="debug">
@@ -27,16 +26,17 @@
<process name="Filter">
<mode name="default">
<message name="IMU_GYRO" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<!-- <message name="IMU_GYRO_LP" period=".017"/> -->
<message name="AHRS_STATE" period=".05"/>
<message name="AHRS_COV" period=".2"/>
<message name="AHRS_MEASURE" period=".05"/>
<message name="IMU_ACCEL" period=".5"/>
<!-- <message name="IMU_ACCEL_RAW" period=".2"/> -->
<message name="IMU_MAG_RAW" period=".5"/>
<message name="IMU_ACCEL" period=".5"/>
<!-- <message name="IMU_GYRO_RAW" period=".05"/> -->
<message name="IMU_GYRO" period=".05"/>
<!-- <message name="IMU_MAG_RAW" period=".5"/> -->
<message name="IMU_MAG" period=".5"/>
<message name="IMU_PRESSURE" period=".5"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/ahrs_comp_filter.c
+1 -1
View File
@@ -30,7 +30,7 @@
#define DT_PREDICT (1./250.)
#define K1_accel 0.05
#define K1_accel 0.075
#define K2_accel 0.00025
#define K1_mag 0.1
#define K2_mag 0.00025
sw/airborne/booz_autopilot.c
+26
View File
@@ -1,3 +1,27 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz_autopilot.h"
#include "radio_control.h"
@@ -23,6 +47,7 @@ void booz_autopilot_periodic_task(void) {
SetCommands(booz_control_commands);
break;
case BOOZ_AP_MODE_ATTITUDE:
case BOOZ_AP_MODE_HEADING_HOLD:
booz_control_attitude_run();
SetCommands(booz_control_commands);
break;
@@ -47,6 +72,7 @@ void booz_autopilot_on_rc_event(void) {
booz_control_rate_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_ATTITUDE:
case BOOZ_AP_MODE_HEADING_HOLD:
booz_control_attitude_read_setpoints_from_rc();
break;
case BOOZ_AP_MODE_NAV:
sw/airborne/booz_autopilot.h
+33 -8
View File
@@ -1,13 +1,38 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ_AUTOPILOT_H
#define BOOZ_AUTOPILOT_H
#include "std.h"
#define BOOZ_AP_MODE_FAILSAFE 0
#define BOOZ_AP_MODE_KILL 1
#define BOOZ_AP_MODE_RATE 2
#define BOOZ_AP_MODE_ATTITUDE 3
#define BOOZ_AP_MODE_NAV 4
#define BOOZ_AP_MODE_FAILSAFE 0
#define BOOZ_AP_MODE_KILL 1
#define BOOZ_AP_MODE_RATE 2
#define BOOZ_AP_MODE_ATTITUDE 3
#define BOOZ_AP_MODE_HEADING_HOLD 4
#define BOOZ_AP_MODE_NAV 5
extern uint8_t booz_autopilot_mode;
@@ -17,9 +42,9 @@ extern void booz_autopilot_on_rc_event(void);
#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2)
#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2)
#define BOOZ_AP_MODE_OF_PPRZ(mode) \
((mode) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_RATE : \
(mode) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_ATTITUDE : \
#define BOOZ_AP_MODE_OF_PPRZ(rc) \
((rc) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_ATTITUDE : \
(rc) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_HEADING_HOLD : \
BOOZ_AP_MODE_NAV )
#endif /* BOOZ_AUTOPILOT_H */
sw/airborne/booz_control.c
+98 -50
View File
@@ -1,8 +1,35 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz_control.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "radio_control.h"
#define BOOZ_CONTROL_MIN_THROTTLE 0.05
float booz_control_p_sp;
float booz_control_q_sp;
float booz_control_r_sp;
@@ -47,6 +74,7 @@ float booz_control_attitude_psi_dgain;
#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30.
#define BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP 45.
#define BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP (1./50.)
void booz_control_init(void) {
@@ -88,25 +116,33 @@ void booz_control_rate_read_setpoints_from_rc(void) {
void booz_control_rate_run(void) {
const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp;
const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p;
booz_control_rate_last_err_p = rate_err_p;
const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p );
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float rate_err_p = booz_estimator_uf_p - booz_control_p_sp;
const float rate_d_err_p = rate_err_p - booz_control_rate_last_err_p;
booz_control_rate_last_err_p = rate_err_p;
const float cmd_p = booz_control_rate_pq_pgain * ( rate_err_p + booz_control_rate_pq_dgain * rate_d_err_p );
const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp;
const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q;
booz_control_rate_last_err_q = rate_err_q;
const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q );
const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
const float rate_err_q = booz_estimator_uf_q - booz_control_q_sp;
const float rate_d_err_q = rate_err_q - booz_control_rate_last_err_q;
booz_control_rate_last_err_q = rate_err_q;
const float cmd_q = booz_control_rate_pq_pgain * ( rate_err_q + booz_control_rate_pq_dgain * rate_d_err_q );
const float rate_err_r = booz_estimator_uf_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
@@ -116,42 +152,54 @@ void booz_control_attitude_read_setpoints_from_rc(void) {
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
#ifndef DISABLE_PSI_CONTROL
booz_control_attitude_psi_sp = rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
#else
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
#endif /* DISABLE_PSI_CONTROL */
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
}
else { /* BOOZ_AP_MODE_HEADING_HOLD */
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE)
booz_control_attitude_psi_sp = booz_estimator_psi;
else {
booz_control_attitude_psi_sp += -rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)*BOOZ_CONTROL_ATTITUDE_DT_UPDATE_SP/MAX_PPRZ;
}
}
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
void booz_control_attitude_run(void) {
const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi +
booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta +
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
#ifndef DISABLE_PSI_CONTROL
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
const float cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
booz_control_attitude_psi_dgain * booz_estimator_r;
#else
const float rate_err_r = booz_estimator_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
#endif /* DISABLE_PSI_CONTROL */
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
if (booz_control_power_sp < BOOZ_CONTROL_MIN_THROTTLE) {
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
}
else {
const float att_err_phi = booz_estimator_phi - booz_control_attitude_phi_sp;
const float cmd_p = booz_control_attitude_phi_theta_pgain * att_err_phi +
booz_control_attitude_phi_theta_dgain * booz_estimator_p ;
const float att_err_theta = booz_estimator_theta - booz_control_attitude_theta_sp;
const float cmd_q = booz_control_attitude_phi_theta_pgain * att_err_theta +
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
float cmd_r;
if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) {
const float rate_err_r = booz_estimator_r - booz_control_r_sp;
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
}
else {
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
booz_control_attitude_psi_dgain * booz_estimator_r;
}
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
booz_control_commands[COMMAND_R] = TRIM_PPRZ((int16_t)cmd_r);
booz_control_commands[COMMAND_THROTTLE] = TRIM_PPRZ((int16_t) (booz_control_power_sp * MAX_PPRZ));
}
}
sw/airborne/booz_control.h
+25 -1
View File
@@ -1,3 +1,27 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ_CONTROL_H
#define BOOZ_CONTROL_H
@@ -35,7 +59,7 @@ extern float booz_control_attitude_psi_dgain;
#define BoozControlAttitudeSetSetPoints(_phi_sp, _theta_sp, _psi_sp, _power_sp) { \
booz_control_attitude_phi_sp = _phi_sp; \
booz_control_attitude_theta_sp = _theta_sp; \
booz_control_attitude_psi_sp = _psi_sp; \
booz_control_attitude_psi_sp = _psi_sp; \
booz_control_power_sp = _power_sp; \
}
sw/airborne/booz_controller_main.c
+24
View File
@@ -1,3 +1,27 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz_controller_main.h"
#include "std.h"
sw/airborne/booz_controller_telemetry.h
+17 -9
View File
@@ -43,15 +43,23 @@
#define PERIODIC_SEND_ACTUATORS() \
DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_RATE_LOOP() \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
&booz_estimator_uf_q, &booz_control_q_sp, \
&booz_estimator_uf_r, &booz_control_r_sp );
#define PERIODIC_SEND_BOOZ_ATT_LOOP() \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
&booz_estimator_theta, &booz_control_attitude_theta_sp, \
&booz_estimator_psi, &booz_control_attitude_psi_sp);
#define PERIODIC_SEND_BOOZ_CONTROL() { \
switch (booz_autopilot_mode) { \
case BOOZ_AP_MODE_RATE: \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
&booz_estimator_uf_q, &booz_control_q_sp, \
&booz_estimator_uf_r, &booz_control_r_sp, \
&booz_control_power_sp); \
break; \
case BOOZ_AP_MODE_ATTITUDE: \
case BOOZ_AP_MODE_HEADING_HOLD: \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_attitude_phi_sp, \
&booz_estimator_theta, &booz_control_attitude_theta_sp, \
&booz_estimator_psi, &booz_control_attitude_psi_sp, \
&booz_control_power_sp); \
break; \
} \
}
#define PERIODIC_SEND_BOOZ_UF_RATES() \
DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
sw/airborne/booz_filter_main.c
+1 -1
View File
@@ -131,5 +131,5 @@ static inline void on_imu_mag( void ) {
}
static inline void on_imu_baro( void ) {
DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
// DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
}
sw/airborne/booz_filter_telemetry.h
+2
View File
@@ -98,6 +98,8 @@
&imu_mag_raw[AXIS_Y], \
&imu_mag_raw[AXIS_Z]);
#define PERIODIC_SEND_IMU_PRESSURE() \
DOWNLINK_SEND_IMU_PRESSURE(&imu_pressure);
/*
*
sw/airborne/booz_imu.h
+1
View File
@@ -98,6 +98,7 @@ extern void booz_imu_periodic(void);
else if (scp1000_status == SCP1000_STA_DATA_AVAILABLE) { \
scp1000_status = SCP1000_STA_WAIT_EOC; \
imu_pressure_raw = scp1000_pressure; \
imu_pressure = imu_pressure_raw * 0.25; \
baro_handler(); \
} \
}
sw/airborne/booz_link_mcu.c
+25 -1
View File
@@ -1,3 +1,27 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "booz_link_mcu.h"
#include "std.h"
@@ -89,4 +113,4 @@ extern void booz_link_mcu_periodic_task( void ) {
booz_link_mcu_status = IMU_NO_LINK;
}
#endif /* >BOOZ_CONTROLLER_MCU */
#endif /* BOOZ_CONTROLLER_MCU */