diff --git a/Makefile b/Makefile
index ca9c5999ed..42ba8970a4 100644
--- a/Makefile
+++ b/Makefile
@@ -134,7 +134,7 @@ ac_h : tools static_h
PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` $(TOOLS)/gen_aircraft.out $(AIRCRAFT)
sim_ac: ac_h sim_sitl
-hard_ac: ac_h ap fbw
+hard_ac: ac_h fbw ap
ac: sim_ac hard_ac
doxygen:
diff --git a/conf/airframes/gorrazoptere_esc_3DMG.xml b/conf/airframes/gorrazoptere_esc_3DMG.xml
new file mode 100644
index 0000000000..08d8c2ddd1
--- /dev/null
+++ b/conf/airframes/gorrazoptere_esc_3DMG.xml
@@ -0,0 +1,124 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+include $(PAPARAZZI_SRC)/conf/autopilot/disc_board.makefile
+
+LOCAL_CFLAGS += -DIMU_3DMG
+
+fbw.CFLAGS += -DRATE_LOOP
+fbw.EXTRA_SRCS += control.c imu.c
+fbw.CFLAGS += -DIMU_RESET_ON_BOOT
+fbw.CFLAGS += -DTIMER1_TOP=0x400
+
+
+
diff --git a/conf/autopilot/disc_board.makefile b/conf/autopilot/disc_board.makefile
new file mode 100644
index 0000000000..a9075c7520
--- /dev/null
+++ b/conf/autopilot/disc_board.makefile
@@ -0,0 +1,24 @@
+ARCHI=avr
+
+ap.ARCHDIR = $(ARCHI)
+ap.ARCH = atmega128
+ap.TARGET = autopilot
+ap.TARGETDIR = autopilot
+ap.LOW_FUSE = a0
+ap.HIGH_FUSE = 99
+ap.EXT_FUSE = ff
+ap.LOCK_FUSE = ff
+ap.srcs = main_ap.c $(SRC_ARCH)/modem.c link_mcu.c $(SRC_ARCH)/link_mcu_ap.c $(SRC_ARCH)/spi_ap.c $(SRC_ARCH)/adc_ap.c gps_ubx.c infrared.c pid.c nav.c $(SRC_ARCH)/uart_ap.c estimator.c if_calib.c mainloop.c cam.c
+
+fbw.ARCHDIR = $(ARCHI)
+fbw.ARCH = atmega128
+fbw.TARGET = fbw
+fbw.TARGETDIR = fbw
+fbw.LOW_FUSE = 2e
+fbw.HIGH_FUSE = cb
+fbw.EXT_FUSE = ff
+fbw.LOCK_FUSE = ff
+fbw.srcs = main_fbw.c $(SRC_ARCH)/ppm.c $(SRC_ARCH)/esc.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_ap.c $(SRC_ARCH)/adc_fbw.c
+
+
+
diff --git a/sw/airborne/adc_fbw.h b/sw/airborne/adc_fbw.h
index f28d362708..df6ad5920e 100644
--- a/sw/airborne/adc_fbw.h
+++ b/sw/airborne/adc_fbw.h
@@ -30,8 +30,6 @@
#include "airframe.h"
-#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
-
#include
@@ -56,6 +54,4 @@ struct adc_buf {
void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s);
-#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2 */
-
#endif /* _ADC_H_ */
diff --git a/sw/airborne/avr/esc.c b/sw/airborne/avr/esc.c
new file mode 100644
index 0000000000..4395af1443
--- /dev/null
+++ b/sw/airborne/avr/esc.c
@@ -0,0 +1,58 @@
+/* $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.
+ *
+ */
+
+
+/** Implementation of command.h */
+
+#include
+#include "command.h"
+
+#define MAX_TICK 0x3FF
+#define MOT_CTL_0 OCR3C
+#define MOT_CTL_1 OCR1A
+#define MOT_CTL_2 OCR3B
+#define MOT_CTL_3 OCR3A
+
+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);
+}
+
+#define SERVO_NEUTRAL(_) 0
+#define COMMAND_(i) MOT_CTL_ ## i
+#define COMMAND(i) COMMAND_(i)
+#define ChopServo(x) (x > MAX_TICK ? MAX_TICK : x)
+
+
+void command_set(const pprz_t values[]) {
+ CommandSet(values); /*Generated from airframe.xml */
+}
diff --git a/sw/airborne/avr/ppm.c b/sw/airborne/avr/ppm.c
index cfdd253053..1002471e6d 100644
--- a/sw/airborne/avr/ppm.c
+++ b/sw/airborne/avr/ppm.c
@@ -58,6 +58,14 @@ volatile bool_t ppm_valid;
#define RestartPpmCycle() { state = 0; sync_start = TCNT2; return; }
+#ifdef TIMER1_TOP
+static volatile uint16_t tmr1_ov_cnt = 0;
+SIGNAL(SIG_OVERFLOW1) {
+ tmr1_ov_cnt += TIMER1_TOP;
+ return;
+}
+#endif
+
SIGNAL( SIG_INPUT_CAPTURE1 )
{
static uint16_t last;
@@ -67,6 +75,9 @@ SIGNAL( SIG_INPUT_CAPTURE1 )
static uint8_t sync_start;
this = ICR1;
+#ifdef TIMER1_TOP
+ this += tmr1_ov_cnt;
+#endif
width = this - last;
last = this;
diff --git a/sw/airborne/avr/ppm.h b/sw/airborne/avr/ppm.h
index 44f501a659..732d2de81c 100644
--- a/sw/airborne/avr/ppm.h
+++ b/sw/airborne/avr/ppm.h
@@ -84,6 +84,12 @@ ppm_init( void )
/* Enable interrupt on input capture */
sbi( TIMSK, TICIE1 );
+
+#ifdef TIMER1_TOP
+ /* Enable timer1 overflow it. */
+ /* needed to increase timer1 count to 16 bits in fast pwm mode (TIMER1_TOP rollover) */
+ sbi( TIMSK, TOIE1 );
+#endif
}
#define PPM_NB_PULSES RADIO_CTL_NB
diff --git a/sw/airborne/avr/servo.c b/sw/airborne/avr/servo.c
index 654cc4fffe..d0f56baa20 100644
--- a/sw/airborne/avr/servo.c
+++ b/sw/airborne/avr/servo.c
@@ -2,7 +2,7 @@
*
* Copied from autopilot (autopilot.sf.net) thanx alot Trammell
* (c) 2002 Trammell Hudson
- * (c) 2003 Pascal Brisset, Antoine Drouin
+ * (c) 2003-2005 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
@@ -24,10 +24,11 @@
*/
+/** Implementation of command.h */
+
#include
#include
-#include "servo.h"
-#include "link_autopilot.h"
+#include "command.h"
#include "airframe.h"
@@ -76,6 +77,8 @@
/* holds the servo pulses width in clock ticks */
static uint16_t servo_widths[_4017_NB_CHANNELS];
+#define COMMAND(i) servo_widths[i]
+
/*
* We use the output compare registers to generate our servo pulses.
* These should be connected to a decade counter that routes the
@@ -91,7 +94,7 @@ static uint16_t servo_widths[_4017_NB_CHANNELS];
* Ideally, you can use two decade counters to drive 20 servos.
*/
void
-servo_init( void )
+command_init( void )
{
uint8_t i;
@@ -190,31 +193,6 @@ the sequence:
servo++;
}
-
-void servo_set_one(uint8_t servo, uint16_t value_us) {
- servo_widths[servo] = ChopServo(CLOCK*value_us);
-}
-
-
-/* void */
-/* servo_transmit(void) { */
-/* uint8_t servo; */
-/* uart_transmit((uint8_t)0); uart_transmit((uint8_t)0); */
-
-/* for(servo = 0; servo < _4017_NB_CHANNELS; servo++) { */
-/* uart_transmit((uint8_t)(servo_widths[servo] >> 8)); */
-/* uart_transmit((uint8_t)(servo_widths[servo] & 0xff)); */
-/* } */
-/* uart_transmit((uint8_t)'\n'); */
-/* } */
-
-
-/*
- *
- * defines how servos react to radio control or autopilot channels
- *
- */
-
-void servo_set(const pprz_t values[]) {
- ServoSet(values); /*Generated from airframe.xml */
+void command_set(const pprz_t values[]) {
+ CommandSet(values); /*Generated from airframe.xml */
}
diff --git a/sw/airborne/avr/timer_ap.h b/sw/airborne/avr/timer_ap.h
index 7ec726d3b3..197ed2ef44 100644
--- a/sw/airborne/avr/timer_ap.h
+++ b/sw/airborne/avr/timer_ap.h
@@ -46,7 +46,7 @@ static inline void timer_init( void ) {
/* Timer0: Modem clock is started in modem.h in ctc mode*/
- /* Timer1 @ Clk/1: System clock, ppm and servos */
+ /* Timer1 @ Clk/1: System clock */
TCCR1A = 0x00;
TCCR1B = 0x01;
diff --git a/sw/airborne/avr/uart_fbw.c b/sw/airborne/avr/uart_fbw.c
index e1737fde2d..8d170731b2 100644
--- a/sw/airborne/avr/uart_fbw.c
+++ b/sw/airborne/avr/uart_fbw.c
@@ -41,7 +41,7 @@ static uint8_t tx_buf[ TX_BUF_SIZE ];
* With 16.0 MHz clock,UBRR=25 => 38400 baud
*
*/
-void uart_init_tx( void ) {
+void uart0_init_tx( void ) {
/* Baudrate is 38.4k */
UBRRH = 0;
UBRRL = 25;
@@ -53,14 +53,14 @@ void uart_init_tx( void ) {
UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);
}
-void uart_init_rx() {
+void uart0_init_rx() {
/* Enable receiver */
UCSRB |= _BV(RXEN);
/* Enable uart receive interrupt */
sbi( UCSRB, RXCIE );
}
-void uart_transmit( unsigned char data ) {
+void uart0_transmit( unsigned char data ) {
if (UCSRB & _BV(TXCIE)) {
/* we are waiting for the last char to be sent : buffering */
if (tx_tail == tx_head + 1) { /* BUF_SIZE = 256 */
@@ -75,26 +75,26 @@ void uart_transmit( unsigned char data ) {
}
}
-void uart_print_hex ( uint8_t c ) {
+void uart0_print_hex ( uint8_t c ) {
const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
uint8_t high = (c & 0xF0)>>4;
uint8_t low = c & 0x0F;
- uart_transmit(hex[high]);
- uart_transmit(hex[low]);
+ uart0_transmit(hex[high]);
+ uart0_transmit(hex[low]);
}
-void uart_print_hex16 ( uint16_t c ) {
+void uart0_print_hex16 ( uint16_t c ) {
uint8_t high = (uint8_t)(c>>8);
uint8_t low = (uint8_t)(c);
- uart_print_hex(high);
- uart_print_hex(low);
+ uart0_print_hex(high);
+ uart0_print_hex(low);
}
-void uart_print_string(const uint8_t* s) {
+void uart0_print_string(const uint8_t* s) {
uint8_t i = 0;
while (s[i]) {
- uart_transmit(s[i]);
+ uart0_transmit(s[i]);
i++;
}
}
diff --git a/sw/airborne/command.h b/sw/airborne/command.h
new file mode 100644
index 0000000000..932d732c0d
--- /dev/null
+++ b/sw/airborne/command.h
@@ -0,0 +1,9 @@
+#ifndef COMMAND_H
+#define COMMAND_H
+
+#include "link_autopilot.h"
+
+extern void command_init( void );
+extern void command_set(const pprz_t values[]);
+
+#endif /* COMMAND_H */
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index baa494a5ed..dbe5bbabee 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -28,7 +28,7 @@
#include "int.h"
#include "timer_fbw.h"
-#include "servo.h"
+#include "command.h"
#include "ppm.h"
#include "spi_fbw_hw.h"
#include "spi_fbw.h"
@@ -116,7 +116,7 @@ inline void radio_control_task(void) {
pitch_dot_dgain = roll_dot_dgain;
control_set_desired(last_radio);
#else
- servo_set(last_radio);
+ command_set(last_radio);
#endif
}
}
@@ -129,7 +129,7 @@ inline void spi_task(void) {
#if defined IMU_ANALOG || defined IMU_3DMG
control_set_desired(from_mega128.channels);
#else
- servo_set(from_mega128.channels);
+ command_set(from_mega128.channels);
#endif
}
}
@@ -151,11 +151,11 @@ int main( void )
while (foo2++);
}
}
- uart_init_tx();
+ uart0_init_tx();
#if defined IMU_3DMG
- uart_init_rx();
+ uart0_init_rx();
#else
- uart_print_string("FBW Booting $Id$\n");
+ uart0_print_string("FBW Booting $Id$\n");
#endif
adc_init();
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf);
@@ -165,7 +165,7 @@ int main( void )
#endif
timer_init();
- servo_init();
+ command_init();
ppm_init();
spi_init();
@@ -207,7 +207,7 @@ int main( void )
if ((mode == MODE_MANUAL && !radio_ok) ||
(mode == MODE_AUTO && !mega128_ok)) {
failsafe_mode = TRUE;
- servo_set(failsafe);
+ command_set(failsafe);
}
if(timer_periodic()) {
@@ -222,12 +222,12 @@ int main( void )
static uint8_t foo = 0;
foo++;
if (!(foo%10)) {
- uart_print_hex16(roll_dot);
- uart_transmit(',');
- uart_print_hex16(pitch_dot);
- uart_transmit(',');
- uart_print_hex16(yaw_dot);
- uart_transmit('\n');
+ uart0_print_hex16(roll_dot);
+ uart0_transmit(',');
+ uart0_print_hex16(pitch_dot);
+ uart0_transmit(',');
+ uart0_print_hex16(yaw_dot);
+ uart0_transmit('\n');
}
}
#endif /* 0 */
@@ -236,10 +236,10 @@ int main( void )
control_run();
if (radio_ok) {
if (last_radio[RADIO_THROTTLE] > 0.1*MAX_PPRZ) {
- servo_set(control_commands);
+ command_set(control_commands);
}
else {
- servo_set(failsafe);
+ command_set(failsafe);
}
}
#endif
diff --git a/sw/airborne/uart_fbw.h b/sw/airborne/uart_fbw.h
index 7f6ba2d021..03cfeeb744 100644
--- a/sw/airborne/uart_fbw.h
+++ b/sw/airborne/uart_fbw.h
@@ -29,13 +29,13 @@
#include "uart_fbw_hw.h"
-void uart_init_tx( void );
-void uart_init_rx( void );
-void uart_transmit( unsigned char data );
+void uart0_init_tx( void );
+void uart0_init_rx( void );
+void uart0_transmit( unsigned char data );
-void uart_print_hex ( uint8_t c );
-void uart_print_hex16 ( uint16_t c );
-void uart_print_string(const uint8_t* s);
-void uart_print_float( const float * f);
+void uart0_print_hex ( uint8_t c );
+void uart0_print_hex16 ( uint16_t c );
+void uart0_print_string(const uint8_t* s);
+void uart0_print_float( const float * f);
#endif
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index ec2d188fd0..c31fe7db5f 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -98,7 +98,7 @@ let parse_command = fun command ->
printf " command_value = %s;\\\n" v;
printf " command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP : SERVO_%s_TRAVEL_DOWN;\\\n" servo servo;
printf " servo_value = SERVO_NEUTRAL(SERVO_%s) + (int16_t)(command_value);\\\n" servo;
- printf " servo_widths[SERVO_%s] = ChopServo(servo_value);\\\n\\\n" servo
+ printf " COMMAND(SERVO_%s) = ChopServo(servo_value);\\\n\\\n" servo
| "let" ->
let var = a "var"
and value = a "value" in
@@ -141,7 +141,7 @@ let parse_section = fun s ->
define "SERVO_MAX_US" (sprintf "%.0ful" max);
nl ()
| "command" ->
- printf "#define ServoSet(values) { \\\n";
+ printf "#define CommandSet(values) { \\\n";
printf " uint16_t servo_value;\\\n";
printf " float command_value;\\\n";
List.iter parse_command (Xml.children s);