diff --git a/conf/airframes/gorrazoptere_esc_3DMG.xml b/conf/airframes/gorrazoptere_esc_3DMG.xml
index 2a4215cb8e..5ab9fb76c6 100644
--- a/conf/airframes/gorrazoptere_esc_3DMG.xml
+++ b/conf/airframes/gorrazoptere_esc_3DMG.xml
@@ -62,6 +62,9 @@
+
+
+
diff --git a/conf/autopilot/disc_board.makefile b/conf/autopilot/disc_board.makefile
index 825b0bd23d..69a57cb695 100644
--- a/conf/autopilot/disc_board.makefile
+++ b/conf/autopilot/disc_board.makefile
@@ -10,4 +10,4 @@ ap.EXT_FUSE = ff
ap.LOCK_FUSE = ff
ap.CFLAGS += -DAP -DFBW -DCONFIG=\"config_discboard.h\"
# ap.srcs = sys_time.c main_fbw_2.c main_ap_2.c main.c
-ap.srcs = $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/adc_ap.c $(SRC_ARCH)/uart_ap.c $(SRC_ARCH)/servos_esc_hw.c main_fbw.c inter_mcu.c pid.c estimator.c if_calib.c nav.c main_ap.c mainloop.c main.c
\ No newline at end of file
+ap.srcs = $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/adc_ap.c $(SRC_ARCH)/uart_ap.c $(SRC_ARCH)/servos_esc_hw.c sys_time.c main_fbw.c inter_mcu.c pid.c estimator.c if_calib.c nav.c main_ap.c mainloop.c main.c
diff --git a/sw/airborne/3dmg.c b/sw/airborne/3dmg.c
index d0b3017d15..ccf5c8a96e 100644
--- a/sw/airborne/3dmg.c
+++ b/sw/airborne/3dmg.c
@@ -24,10 +24,6 @@
#include "airframe.h"
-#ifdef IMU_3DMG
-
-#warning "Compiling 3dmg.c"
-
#include "std.h"
#include "3dmg.h"
#include "uart_fbw.h"
@@ -167,5 +163,3 @@ static inline void on_3dmg_receive(uint8_t c) {
ReceiveUart(on_3dmg_receive);
#endif /* AVR_ARCH */
-
-#endif // IMU_3DMG
diff --git a/sw/airborne/adc_ap.h b/sw/airborne/adc_ap.h
index 577322f050..e980362f1f 100644
--- a/sw/airborne/adc_ap.h
+++ b/sw/airborne/adc_ap.h
@@ -25,7 +25,7 @@
*
*/
-/** \file adc.h
+/** \file adc_ap.h
* \brief Analog to Digital Converter API
*
* Facility to store last values in a circular buffer for a specific
diff --git a/sw/airborne/avr/i2c_ap.c b/sw/airborne/avr/i2c_ap.c
index 1a322e8440..5102a6e496 100644
--- a/sw/airborne/avr/i2c_ap.c
+++ b/sw/airborne/avr/i2c_ap.c
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*
*/
-/** \file i2c.c
+/** \file i2c_ap.c
* \brief Basic library for I2C
*
*/
diff --git a/sw/airborne/avr/i2c_ap.h b/sw/airborne/avr/i2c_ap.h
index 3f7ad4e8e6..5c8e4d2805 100644
--- a/sw/airborne/avr/i2c_ap.h
+++ b/sw/airborne/avr/i2c_ap.h
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*
*/
-/** \file i2c.h
+/** \file i2c_ap.h
* \brief Basic library for I2C
*
*/
diff --git a/sw/airborne/avr/ppm_hw.c b/sw/airborne/avr/ppm_hw.c
index 79ff386bb2..ddca3d4861 100644
--- a/sw/airborne/avr/ppm_hw.c
+++ b/sw/airborne/avr/ppm_hw.c
@@ -22,6 +22,7 @@
* Boston, MA 02111-1307, USA.
*
*/
+
#include
#include "ppm.h"
#include "sys_time.h"
diff --git a/sw/airborne/avr/servos_esc_hw.c b/sw/airborne/avr/servos_esc_hw.c
index 46b5da6104..8f4338cd84 100644
--- a/sw/airborne/avr/servos_esc_hw.c
+++ b/sw/airborne/avr/servos_esc_hw.c
@@ -51,6 +51,19 @@
const pprz_t failsafe_values[COMMANDS_NB] = COMMANDS_FAILSAFE;
+void command_init ( void ) {
+ /* OC1A output */
+ DDRB |= _BV(5);
+ /* fast PWM, 10 bits */
+ TCCR1A |= _BV(WGM10) | _BV(WGM11) | _BV(COM1A1);
+ TCCR1B |= _BV(WGM12);
+ /* OC3A, OC3B, OC3C outputs */
+ DDRE |= _BV(3) | _BV(4) | _BV(5);
+ /* fast PWM : 10 bits */
+ TCCR3A |= _BV(WGM30) | _BV(WGM31) | _BV(COM3A1) | _BV(COM3B1) | _BV(COM3C1);
+ TCCR3B |= _BV(WGM32);
+}
+
void command_set(const pprz_t values[]) {
CommandsSet(values); /*Generated from airframe.xml */
}
diff --git a/sw/airborne/avr/servos_esc_hw.h b/sw/airborne/avr/servos_esc_hw.h
index 2ef6b73c38..712e85b519 100644
--- a/sw/airborne/avr/servos_esc_hw.h
+++ b/sw/airborne/avr/servos_esc_hw.h
@@ -36,17 +36,4 @@
#include
-static inline void command_init ( void ) {
- /* OC1A output */
- DDRB |= _BV(5);
- /* fast PWM, 10 bits */
- TCCR1A |= _BV(WGM10) | _BV(WGM11) | _BV(COM1A1);
- TCCR1B |= _BV(WGM12);
- /* OC3A, OC3B, OC3C outputs */
- DDRE |= _BV(3) | _BV(4) | _BV(5);
- /* fast PWM : 10 bits */
- TCCR3A |= _BV(WGM30) | _BV(WGM31) | _BV(COM3A1) | _BV(COM3B1) | _BV(COM3C1);
- TCCR3B |= _BV(WGM32);
-}
-
#endif /* SERVOS_ESC_HW_H */
diff --git a/sw/airborne/command.h b/sw/airborne/command.h
index d5545c518b..15d23d0ff0 100644
--- a/sw/airborne/command.h
+++ b/sw/airborne/command.h
@@ -1,8 +1,30 @@
+/* $Id$
+ *
+ * Copied from autopilot (autopilot.sf.net) thanx alot Trammell
+ * (c) 2003-2005 Pascal Brisset, 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 COMMAND_H
#define COMMAND_H
-//#include "inter_mcu.h"
-
#include "paparazzi.h"
extern void command_init( void );
diff --git a/sw/airborne/control_grz.c b/sw/airborne/control_grz.c
index e57d657c5b..158a7384ca 100644
--- a/sw/airborne/control_grz.c
+++ b/sw/airborne/control_grz.c
@@ -22,92 +22,80 @@
*
*/
+#include "std.h"
#include "airframe.h"
-
-
#include "control_grz.h"
-#include "estimator.h"
-#include "radio.h"
+#include "imu.h"
+#include "main_fbw.h"
-int16_t setpoint_roll = 0;
-float roll_pgain = -0.8;
+#define NORMALIZE(n, lower, uper) { \
+ while ( n > uper) { n -= (uper - lower);} \
+ while ( n < lower) { n += (uper - lower);} \
+}
+
+float fbw_roll_pgain = -0.8;
#define MAX_ROLL_DOT (1. * 32768000. / 8500.)
-int16_t setpoint_pitch = 0;
-float pitch_pgain = -0.5;
+float fbw_pitch_pgain = -0.5;
#define MAX_PITCH_DOT (1. * 32768000. / 8500.)
-int16_t setpoint_yaw = 0;
-float yaw_pgain = -2.5;
+float fbw_yaw_pgain = -2.5;
#define MAX_YAW_DOT (1. * 32768000. / 8500.)
-int16_t setpoint_roll_dot = 0;
float roll_dot_pgain = -1.9;
float roll_dot_igain = 0.0;
float roll_dot_dgain = 0.0;
int16_t roll_dot_sum_err = 0;
int16_t roll_dot_last_err = 0;
-pprz_t command_roll = 0;
-int16_t setpoint_pitch_dot = 0;
float pitch_dot_pgain = -1.9;
float pitch_dot_igain = 0.0;
float pitch_dot_dgain = 0.0;
int16_t pitch_dot_sum_err = 0;
int16_t pitch_dot_last_err = 0;
-pprz_t command_pitch = 0;
-int16_t setpoint_yaw_dot = 0;
float yaw_dot_pgain = -4.8;
float yaw_dot_igain = 0.0;
float yaw_dot_dgain = 0.0;
int16_t yaw_dot_sum_err = 0;
int16_t yaw_dot_last_err = 0;
-pprz_t command_yaw = 0;
-int16_t setpoint_throttle = 0;
-pprz_t command_throttle = 0;
-
-pprz_t control_commands[] = {0, 0, 0, 0};
void control_rate_run ( void ) {
- int16_t err = e_roll_dot - setpoint_roll_dot;
+ int16_t err = roll_dot - commands[COMMAND_PHI_DOT];
int16_t d_err = err - roll_dot_last_err;
roll_dot_last_err = err;
// roll_dot_sum_err += err;
// control_commands[RADIO_ROLL] = TRIM_PPRZ((err + d_err * roll_dot_dgain + roll_dot_sum_err * roll_dot_igain)
- control_commands[RADIO_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_dgain) * roll_dot_pgain));
+ commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_dgain) * roll_dot_pgain));
- err = e_pitch_dot - setpoint_pitch_dot;
+ err = pitch_dot - commands[COMMAND_THETA_DOT];
d_err = err - pitch_dot_last_err;
pitch_dot_last_err = err;
// pitch_dot_sum_err += err;
// control_commands[RADIO_PITCH] = TRIM_PPRZ((err + d_err * pitch_dot_dgain + pitch_dot_sum_err * pitch_dot_igain) * pitch_dot_pgain);
- control_commands[RADIO_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_dgain) * pitch_dot_pgain));
+ commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_dgain) * pitch_dot_pgain));
- err = e_yaw_dot - setpoint_yaw_dot;
+ err = yaw_dot - commands[COMMAND_PSI_DOT];
// d_err = err - yaw_dot_last_err;
// yaw_dot_last_err = err;
// yaw_dot_sum_err += err;
// control_commands[RADIO_YAW] = TRIM_PPRZ((err + d_err * yaw_dot_dgain + yaw_dot_sum_err * yaw_dot_igain) * yaw_dot_pgain);
- control_commands[RADIO_YAW] = TRIM_PPRZ((int16_t)(err * yaw_dot_pgain));
-
- control_commands[RADIO_THROTTLE] = TRIM_PPRZ(setpoint_throttle);
+ commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)(err * yaw_dot_pgain));
}
-
void control_attitude_run ( void ) {
- int16_t err = e_roll - setpoint_roll;
- setpoint_roll_dot = TRIM(roll_pgain * err, -MAX_ROLL_DOT, MAX_ROLL_DOT);
+ int16_t err = roll - commands[COMMAND_PHI];
+ commands[COMMAND_PHI_DOT] = Chop(fbw_roll_pgain * err, -MAX_ROLL_DOT, MAX_ROLL_DOT);
- err = e_pitch - setpoint_pitch;
- setpoint_pitch_dot = TRIM(pitch_pgain * err, -MAX_PITCH_DOT, MAX_PITCH_DOT);
+ err = pitch - commands[COMMAND_THETA];
+ commands[COMMAND_THETA_DOT] = Chop(fbw_pitch_pgain * err, -MAX_PITCH_DOT, MAX_PITCH_DOT);
}
void control_heading_run ( void ) {
- int16_t err = - (e_yaw/4 - setpoint_yaw/4);
+ int16_t err = - (yaw/4 - commands[COMMAND_PSI]/4);
NORMALIZE(err, -8192, 8192);
- setpoint_yaw_dot = TRIM(yaw_pgain * err, -MAX_YAW_DOT, MAX_YAW_DOT);
+ commands[COMMAND_PSI_DOT] = Chop(fbw_yaw_pgain * err, -MAX_YAW_DOT, MAX_YAW_DOT);
}
-#endif
+
diff --git a/sw/airborne/control_grz.h b/sw/airborne/control_grz.h
index f57e12790e..4fc67672dd 100644
--- a/sw/airborne/control_grz.h
+++ b/sw/airborne/control_grz.h
@@ -27,31 +27,19 @@
#include
-extern int16_t setpoint_roll;
-extern float roll_pgain;
+extern float fbw_roll_pgain;
+extern float fbw_pitch_pgain;
+extern float fbw_yaw_pgain;
-extern int16_t setpoint_pitch;
-extern float pitch_pgain;
-
-extern int16_t setpoint_yaw;
-extern float yaw_pgain;
-
-extern int16_t setpoint_roll_dot;
extern float roll_dot_pgain;
extern float roll_dot_dgain;
-extern int16_t setpoint_pitch_dot;
extern float pitch_dot_pgain;
extern float pitch_dot_dgain;
-extern int16_t setpoint_yaw_dot;
extern float yaw_dot_dgain;
extern float yaw_dot_pgain;
-extern int16_t setpoint_throttle;
-
-extern pprz_t control_commands[];
-
void control_rate_run ( void );
void control_attitude_run ( void );
void control_heading_run ( void );
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index 3b7719c05f..314dda62c0 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/estimator.c
@@ -94,9 +94,9 @@ void estimator_init( void ) {
#ifdef IMU_3DMG
#include "inter_mcu.h"
void estimator_update_state_3DMG( void ) {
- estimator_phi = from_fbw.euler[0];
- estimator_psi = from_fbw.euler[1];
- estimator_theta = from_fbw.euler[2];
+ estimator_phi = from_fbw.from_fbw.euler[0];
+ estimator_psi = from_fbw.from_fbw.euler[1];
+ estimator_theta = from_fbw.from_fbw.euler[2];
}
#elif defined IMU_ANALOG && defined AHRS
#include "ahrs.h"
diff --git a/sw/airborne/if_calib.c b/sw/airborne/if_calib.c
index 7ca5ebf166..4608cb10b1 100644
--- a/sw/airborne/if_calib.c
+++ b/sw/airborne/if_calib.c
@@ -43,5 +43,7 @@
#include "estimator.h"
+#define RcChannel(x) (from_fbw.from_fbw.channels[x])
+
/** Includes generated code from airframe.xml */
#include "inflight_calib.h"
diff --git a/sw/airborne/imu.c b/sw/airborne/imu.c
index 763c789805..f64d9bfef5 100644
--- a/sw/airborne/imu.c
+++ b/sw/airborne/imu.c
@@ -33,7 +33,6 @@
int16_t roll_dot, pitch_dot, yaw_dot;
#ifdef IMU_3DMG
-#warning "Compiling imu.c for 3DMG"
#include "3dmg.h"
int16_t roll, pitch, yaw;
@@ -58,7 +57,6 @@ void imu_capture_neutral ( void ) {
#endif // 3DMG
#ifdef IMU_ANALOG
-#warning "Compiling imu.c for ANALOG"
#include "adc_fbw.h"
struct adc_buf buf_roll_dot;
diff --git a/sw/airborne/inter_mcu.h b/sw/airborne/inter_mcu.h
index 291c1069fa..e5c8ada235 100644
--- a/sw/airborne/inter_mcu.h
+++ b/sw/airborne/inter_mcu.h
@@ -46,13 +46,6 @@
/* !!!!!!!!!!!!!!!!!!! Value used in gen_airframe.ml !!!!!!!!!!!!!!!!! */
-#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \
- (pprz > MAX_PPRZ ? MAX_PPRZ : \
- pprz))
-#define TRIM_UPPRZ(pprz) (pprz < 0 ? 0 : \
- (pprz > MAX_PPRZ ? MAX_PPRZ : \
- pprz))
-
/** Data structure shared by fbw and ap sub-mofules */
struct from_fbw_msg {
pprz_t channels[RADIO_CTL_NB];
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index d0fe955b98..accb0e912a 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -22,10 +22,6 @@
*
*/
-
-
-#include
-
#include "main_fbw.h"
#include "int.h"
//#include "timer_fbw.h"
@@ -35,7 +31,6 @@
#include "inter_mcu.h"
#define RC_AVG_PERIOD 8
-#include "radio.h"
#include "led.h"
#include "uart_fbw.h"
@@ -75,7 +70,7 @@ static uint8_t ppm_cpt, last_ppm_cpt;
#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer
-static pprz_t commands[COMMANDS_NB];
+pprz_t commands[COMMANDS_NB];
/** Storage of intermediate command values: these values come from
the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
They are asyncronisly used to set the servos */
@@ -189,7 +184,6 @@ void init_fbw( void ) {
int_enable();
#if IMU_RESET_ON_BOOT
-#warning IMU_RESET_ON_BOOT
imu_capture_neutral();
#endif
}
@@ -214,11 +208,7 @@ void event_task_fbw( void) {
time_since_last_ap = 0;
ap_ok = TRUE;
if (mode == FBW_MODE_AUTO) {
-#if defined IMU_ANALOG || defined IMU_3DMG
- control_set_desired(from_ap.from_ap.channels);
-#else
SetCommands(from_ap.from_ap.channels);
-#endif
}
to_autopilot_from_rc_values();
}
@@ -257,7 +247,7 @@ void periodic_task_fbw( void ) {
imu_update();
#endif
#if defined IMU_3DMG || defined IMU_ANALOG
- control_run(commands);
+ control_rate_run();
if (radio_ok) {
if (rc_values[RADIO_THROTTLE] < 0.1*MAX_PPRZ) {
SetCommands(failsafe);
diff --git a/sw/airborne/main_fbw.h b/sw/airborne/main_fbw.h
index 16e81afbcc..ff9fd859c9 100644
--- a/sw/airborne/main_fbw.h
+++ b/sw/airborne/main_fbw.h
@@ -25,6 +25,10 @@
#ifndef FBW_H
#define FBW_H
+#include "paparazzi.h"
+#include "airframe.h"
+
+extern pprz_t commands[COMMANDS_NB];
extern void init_fbw( void );
extern void periodic_task_fbw( void );
extern void event_task_fbw( void );
diff --git a/sw/airborne/paparazzi.h b/sw/airborne/paparazzi.h
index c7ce89090c..57c0db8f26 100644
--- a/sw/airborne/paparazzi.h
+++ b/sw/airborne/paparazzi.h
@@ -8,7 +8,11 @@ typedef int16_t pprz_t; /* type of commands */
#define MAX_PPRZ 9600
#define MIN_PPRZ -MAX_PPRZ
-
-
+#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \
+ (pprz > MAX_PPRZ ? MAX_PPRZ : \
+ pprz))
+#define TRIM_UPPRZ(pprz) (pprz < 0 ? 0 : \
+ (pprz > MAX_PPRZ ? MAX_PPRZ : \
+ pprz))
#endif /* PAPARAZZI_H */
diff --git a/sw/airborne/ppm.h b/sw/airborne/ppm.h
index 3398f06725..85b57d0e77 100644
--- a/sw/airborne/ppm.h
+++ b/sw/airborne/ppm.h
@@ -24,7 +24,6 @@
#ifndef PPM_H
#define PPM_H
-#include
#include "std.h"
#include "radio.h"
diff --git a/sw/tools/gen_calib.ml b/sw/tools/gen_calib.ml
index 0a668f889f..180427788a 100644
--- a/sw/tools/gen_calib.ml
+++ b/sw/tools/gen_calib.ml
@@ -67,9 +67,9 @@ let parse_setting = fun xml ->
lprintf "if (mode_changed) {\n";
right ();
lprintf "%s = %s;\n" var_init var;
- lprintf "slider%d_init = from_fbw.channels[RADIO_GAIN%d];\n" cursor cursor;
+ lprintf "slider%d_init = RcChannel(RADIO_GAIN%d);\n" cursor cursor;
left (); lprintf "}\n";
- lprintf "%s = %s(%s, %f, from_fbw.channels[RADIO_GAIN%d], slider%d_init);\n" var param_macro var_init range cursor cursor;
+ lprintf "%s = %s(%s, %f, RcChannel(RADIO_GAIN%d), slider%d_init);\n" var param_macro var_init range cursor cursor;
lprintf "slider_%d_val = (float)%s;\n" cursor var;
left (); lprintf "}\n"