diff --git a/conf/airframes/plaster1.xml b/conf/airframes/plaster1.xml index 3ecd95679d..9aa7f84303 100755 --- a/conf/airframes/plaster1.xml +++ b/conf/airframes/plaster1.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/autopilot/twin_avr.makefile b/conf/autopilot/twin_avr.makefile index ca75a5d622..4a2645d798 100644 --- a/conf/autopilot/twin_avr.makefile +++ b/conf/autopilot/twin_avr.makefile @@ -20,4 +20,4 @@ fbw.HIGH_FUSE = cb fbw.EXT_FUSE = ff fbw.LOCK_FUSE = ff fbw.CFLAGS += -DFBW -DMCU_SPI_LINK -DACTUATORS=\"servos_4017.h\" -fbw.srcs = $(SRC_ARCH)/ppm_hw.c inter_mcu.c $(SRC_ARCH)/servos_4017.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_fbw.c $(SRC_ARCH)/adc_fbw.c sys_time.c main_fbw.c main.c +fbw.srcs = $(SRC_ARCH)/ppm_hw.c commands.c inter_mcu.c $(SRC_ARCH)/servos_4017.c $(SRC_ARCH)/spi_fbw.c $(SRC_ARCH)/uart_fbw.c $(SRC_ARCH)/adc_fbw.c sys_time.c main_fbw.c main.c diff --git a/conf/conf.xml.example b/conf/conf.xml.example index 6ff50775de..43c2bd2b76 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -19,12 +19,12 @@ gui_color="#800080" /> - diff --git a/sw/airborne/actuators.h b/sw/airborne/actuators.h index 6d95b23376..c78e1f0e7d 100644 --- a/sw/airborne/actuators.h +++ b/sw/airborne/actuators.h @@ -26,11 +26,14 @@ #if defined ACTUATORS #include "paparazzi.h" -extern const pprz_t failsafe_values[]; + +#include "airframe.h" +/** Defines SetActuatorsFromCommands() macro */ + extern void actuators_init( void ); -extern void actuators_set(const pprz_t values[]); -#include ACTUATORS /* ACTUATORS */ +#include ACTUATORS +#endif /* ACTUATORS */ -#endif ACTUATORS_H +#endif /* ACTUATORS_H */ diff --git a/sw/airborne/arm7/servos_4015_hw.c b/sw/airborne/arm7/servos_4015_hw.c index 41334061fa..cbd33137d8 100644 --- a/sw/airborne/arm7/servos_4015_hw.c +++ b/sw/airborne/arm7/servos_4015_hw.c @@ -61,7 +61,3 @@ void PWM_ISR ( void ) { VICVectAddr = 0x00000000; ISR_EXIT(); } - -void command_set(const pprz_t values[]) { - CommandsSet(values); /*Generated from airframe.xml */ -} diff --git a/sw/airborne/avr/adc_fbw.c b/sw/airborne/avr/adc_fbw.c index 39a5b26ba2..cb62a51280 100644 --- a/sw/airborne/avr/adc_fbw.c +++ b/sw/airborne/avr/adc_fbw.c @@ -98,7 +98,7 @@ SIGNAL( SIG_ADC ) if (buf) { uint8_t new_head = buf->head + 1; - if (new_head >= AV_NB_SAMPLE) new_head = 0; + if (new_head >= buf->av_nb_sample) new_head = 0; buf->sum -= buf->values[new_head]; buf->values[new_head] = adc_value; buf->sum += adc_value; diff --git a/sw/airborne/avr/ppm_hw.c b/sw/airborne/avr/ppm_hw.c index ddca3d4861..5d37afce60 100644 --- a/sw/airborne/avr/ppm_hw.c +++ b/sw/airborne/avr/ppm_hw.c @@ -77,7 +77,7 @@ SIGNAL( SIG_INPUT_CAPTURE1 ) last = this; if( state == 0 ) { - uint8_t end = TCNT2; + uint8_t end = TCNT2; uint8_t diff = (end - sync_start); sync_start = end; @@ -99,7 +99,7 @@ SIGNAL( SIG_INPUT_CAPTURE1 ) ppm_pulses[state - 1] = width; if (state >= PPM_NB_PULSES) { - ppm_valid = 1; + ppm_valid = TRUE; RestartPpmCycle(); } else state++; diff --git a/sw/airborne/avr/servos_4017.c b/sw/airborne/avr/servos_4017.c index 11a5434606..27bd738af9 100644 --- a/sw/airborne/avr/servos_4017.c +++ b/sw/airborne/avr/servos_4017.c @@ -26,20 +26,15 @@ #include #include -#include "command.h" +#include "servos_4017.h" +#include "actuators.h" #include "sys_time.h" -#include "airframe.h" #include CONFIG -#define _4017_NB_CHANNELS 10 /* holds the servo pulses width in clock ticks */ -static uint16_t servo_widths[_4017_NB_CHANNELS]; -static const pprz_t failsafe_values[COMMANDS_NB] = COMMANDS_FAILSAFE; +uint16_t servo_widths[_4017_NB_CHANNELS]; -#define ChopServo(x,a,b) Chop(x, a, b) - -#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 @@ -54,7 +49,7 @@ static const pprz_t failsafe_values[COMMANDS_NB] = COMMANDS_FAILSAFE; * * Ideally, you can use two decade counters to drive 20 servos. */ -void command_init( void ) { +void actuators_init( void ) { uint8_t i; /* Configure the reset and clock lines as output */ _4017_RESET_DDR |= _BV(_4017_RESET_PIN); @@ -66,8 +61,6 @@ void command_init( void ) { /* Set all servos at their midpoints */ for( i=0 ; i < _4017_NB_CHANNELS ; i++ ) servo_widths[i] = SYS_TICS_OF_USEC(1500); - /* Load the failsafe defaults */ - command_set(failsafe_values); /* Set servos to go off some long time from now */ SERVO_OCR = 32768ul; /* Set output compare to toggle the output bits */ @@ -140,7 +133,3 @@ the sequence: servo++; } - -void command_set(const pprz_t values[]) { - CommandsSet(values); /*Generated from airframe.xml */ -} diff --git a/sw/airborne/avr/servos_4017.h b/sw/airborne/avr/servos_4017.h index e678f2e76f..4f27180952 100644 --- a/sw/airborne/avr/servos_4017.h +++ b/sw/airborne/avr/servos_4017.h @@ -1 +1,36 @@ +/* $Id$ + * + * (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 SERVO_4017_H +#define SERVO_4017_H + #define SERVOS_TICS_OF_USEC(_us) SYS_TICS_OF_USEC(_us) +#define ChopServo(x,a,b) Chop(x, a, b) + +#define _4017_NB_CHANNELS 10 +extern uint16_t servo_widths[_4017_NB_CHANNELS]; +#define Actuator(i) servo_widths[i] + +#endif /* SERVO_4017_H */ + + diff --git a/sw/airborne/avr/sys_time_hw.h b/sw/airborne/avr/sys_time_hw.h index 7710c39373..fbb6ac36cf 100644 --- a/sw/airborne/avr/sys_time_hw.h +++ b/sw/airborne/avr/sys_time_hw.h @@ -46,11 +46,18 @@ static inline void sys_time_init( void ) { /* Timer0: Modem clock is started in modem.h in ctc mode*/ /* Timer1 @ Clk/1: System clock */ - TCCR1A = 0x00; - TCCR1B = 0x01; + TCCR1A = 0x00; + TCCR1B = _BV(CS10); /* Timer2 @ Clk/1024: Periodic clock */ - TCCR2 = 0x05; + +#if defined (__AVR_ATmega8__) + TCCR2 = _BV(CS20) | _BV(CS21) | _BV(CS22); +#elif defined (__AVR_ATmega128__) + TCCR2 = _BV(CS20) | _BV(CS22); +#else +#warning "Unknown arch" +#endif cpu_time = 0; } @@ -68,7 +75,7 @@ static inline void sys_time_init( void ) { static inline bool_t sys_time_periodic( void ) { if( !bit_is_set( TIFR, TOV2 ) ) return FALSE; - TIFR = 1 << TOV2; + TIFR = _BV(TOV2); return TRUE; } diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c index ca097a1e4b..483779570a 100644 --- a/sw/airborne/infrared.c +++ b/sw/airborne/infrared.c @@ -64,8 +64,6 @@ static struct adc_buf buf_ir2; #ifndef ADC_CHANNEL_IR_NB_SAMPLES #define ADC_CHANNEL_IR_NB_SAMPLES DEFAULT_AV_NB_SAMPLE -#else -#warning " ADC_CHANNEL_IR_NB_SAMPLES"; #endif /** \brief Initialisation of \a ir */ /** Initialize \a ir with the \a IR_DEFAULT_CONTRAST \n diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c index 2ae14fe4ba..c7bbf1d609 100644 --- a/sw/airborne/main_fbw.c +++ b/sw/airborne/main_fbw.c @@ -26,10 +26,11 @@ #include "int.h" #include "sys_time.h" #include "actuators.h" +#include "commands.h" #include "ppm.h" #include "inter_mcu.h" -#define RC_AVG_PERIOD 8 + #include "led.h" #include "uart_fbw.h" @@ -61,25 +62,12 @@ static pprz_t rc_values[ PPM_NB_PULSES ]; static pprz_t avg_rc_values[ PPM_NB_PULSES ]; static bool_t rc_values_contains_avg_channels = FALSE; -static const pprz_t failsafe[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - static uint8_t ppm_cpt, last_ppm_cpt; #define STALLED_TIME 30 // 500ms with a 60Hz timer #define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer -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 */ - -#define SetCommands(t) { \ - int i; \ - for(i = 0; i < COMMANDS_NB; i++) commands[i] = t[i]; \ -} - - /* Prepare data to be sent to mcu0 */ static inline void to_autopilot_from_rc_values (void) { struct from_fbw_msg *msg = &(from_fbw.from_fbw); @@ -113,6 +101,8 @@ static inline void to_autopilot_from_rc_values (void) { #endif } + +#define RC_AVG_PERIOD 8 /* Copy from the ppm receiving buffer to the buffer sent to mcu0 */ static void rc_values_from_ppm( void ) { NormalizePpm(); @@ -143,7 +133,7 @@ static inline void radio_control_task(void) { pitch_dot_dgain = roll_dot_dgain; #endif - CommandsOfRC(commands); + SetCommandsFromRC(commands); } } @@ -173,7 +163,11 @@ void init_fbw( void ) { #endif sys_time_init(); - command_init(); + actuators_init(); + + /* Load the failsafe defaults */ + SetCommands(commands_failsafe); + ppm_init(); #ifdef MCU_SPI_LINK @@ -234,7 +228,7 @@ void event_task_fbw( void) { if ((mode == FBW_MODE_MANUAL && !radio_ok) || (mode == FBW_MODE_AUTO && !ap_ok)) { failsafe_mode = TRUE; - SetCommands(failsafe); + SetCommands(commands_failsafe); } } @@ -258,6 +252,14 @@ void periodic_task_fbw( void ) { _1Hz = 0; last_ppm_cpt = ppm_cpt; ppm_cpt = 0; +#ifdef DEBUG + uint8_t i; + for(i = 0; i < 4; i++) { + uart0_print_hex16(commands[i]); + uart0_print_string(" "); + } + uart0_print_string("\n"); +#endif } if (time_since_last_ap < STALLED_TIME) @@ -266,6 +268,5 @@ void periodic_task_fbw( void ) { if (time_since_last_ppm < REALLY_STALLED_TIME) time_since_last_ppm++; - /** Set servo values */ - command_set(commands); + // SetActuatorsFromCommands(commands); } diff --git a/sw/airborne/main_fbw.h b/sw/airborne/main_fbw.h index ff9fd859c9..3b0a3fecda 100644 --- a/sw/airborne/main_fbw.h +++ b/sw/airborne/main_fbw.h @@ -28,7 +28,6 @@ #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/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml index 3a0f1ac374..1edd9e966c 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -96,7 +96,7 @@ let parse_command_laws = 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_%s_NEUTRAL + (int16_t)(command_value);\\\n" servo; - printf " COMMAND(SERVO_%s) = SERVOS_TICS_OF_USEC(ChopServo(servo_value, SERVO_%s_MIN, SERVO_%s_MAX));\\\n\\\n" servo servo servo + printf " Actuator(SERVO_%s) = SERVOS_TICS_OF_USEC(ChopServo(servo_value, SERVO_%s_MIN, SERVO_%s_MAX));\\\n\\\n" servo servo servo | "let" -> let var = a "var" and value = a "value" in @@ -140,8 +140,8 @@ let parse_section = fun s -> nl () | "servos" -> let servos = Xml.children s in - let nb_servos = List.fold_right (fun s m -> Pervasives.max (int_of_string (ExtXml.attrib s "no")) m) servos min_int in - define "LAST_SERVO_CHANNEL" (string_of_int nb_servos); + let nb_servos = List.fold_right (fun s m -> Pervasives.max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in + define "SERVOS_NB" (string_of_int nb_servos); nl (); List.iter parse_servo servos; nl () @@ -152,11 +152,11 @@ let parse_section = fun s -> define "COMMANDS_FAILSAFE" (sprint_float_array (List.map (fun x -> string_of_int x.failsafe_value) (Array.to_list commands_params))); nl (); nl () | "rc_commands" -> - printf "#define CommandsOfRC(commands) { \\\n"; + printf "#define SetCommandsFromRC(commands) { \\\n"; List.iter parse_rc_commands (Xml.children s); printf "}\n\n" | "command_laws" -> - printf "#define ActuatorsSet(values) { \\\n"; + printf "#define SetActuatorsFromCommands(values) { \\\n"; printf " uint16_t servo_value;\\\n"; printf " float command_value;\\\n"; List.iter parse_command_laws (Xml.children s);