diff --git a/sw/airborne/fly_by_wire/main.c b/sw/airborne/fly_by_wire/main.c index 5bfe6d6ad7..f6e8d1e57a 100644 --- a/sw/airborne/fly_by_wire/main.c +++ b/sw/airborne/fly_by_wire/main.c @@ -118,7 +118,7 @@ int main( void ) ppm_cpt++; radio_ok = TRUE; radio_really_lost = FALSE; - time_since_last_ppm = 0; + // time_since_last_ppm = 0; last_radio_from_ppm(); if (last_radio_contains_avg_channels) { mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]); diff --git a/sw/airborne/fly_by_wire/ppm.c b/sw/airborne/fly_by_wire/ppm.c index adeae0fbd7..7100503f49 100644 --- a/sw/airborne/fly_by_wire/ppm.c +++ b/sw/airborne/fly_by_wire/ppm.c @@ -49,6 +49,8 @@ pprz_t last_radio[ PPM_NB_PULSES ]; pprz_t avg_last_radio[ PPM_NB_PULSES ]; bool_t last_radio_contains_avg_channels = FALSE; volatile bool_t ppm_valid; +uint8_t ppm_status; +uint16_t ppm_time_since_last_valid; /* MC3030, Trame PPM7: 25ms, 10.4 au neutre, sync pulse = 16.2ms with low value on every channels */ @@ -77,9 +79,9 @@ SIGNAL( SIG_INPUT_CAPTURE1 ) /* The frame period of the mc3030 seems to be 25ms. * One pulse lasts from 1.05ms to 2.150ms. - * Sync pulse is at least 7ms : (7000*CLOCK)/1024 = 109 + * Sync pulse is at least 5.5ms : (5500*CLOCK)/1024 = 109 */ - if( diff > (uint8_t)(((uint32_t)(7000ul*CLOCK))/1024ul) ) { + if( diff > (uint8_t)(((uint32_t)(5500ul*CLOCK))/1024ul) ) { state = 1; } } @@ -98,7 +100,8 @@ SIGNAL( SIG_INPUT_CAPTURE1 ) return; } -#define Int16FromPulse(i) (int16_t)((ppm_pulses[(i)] - PpmOfUs(((int[])RADIO_NEUTRALS_US)[i]))*(2*MAX_PPRZ)/(PpmOfUs(((int[])RADIO_MAXS_US[i])-((int[])RADIO_MINS_US[i])))) +#define Int16FromPulse(i) (int16_t)((ppm_pulses[(i)] - PpmOfUs(((uint16_t[])RADIO_NEUTRALS_US)[i]))* (2*MAX_PPRZ)/(PpmOfUs(((uint16_t[])RADIO_MAXS_US[i])-((uint16_t[])RADIO_MINS_US[i])))) +#define NewInt16FromPulse(i) (int16_t)((ppm_pulses[(i)] - ((uint16_t[])RADIO_NEUTRALS_PPM)[i])) * (float[])RADIO_TRAVEL_PPM[i] /* Copy from the ppm receiving buffer to the buffer sent to mcu0 */ @@ -107,7 +110,7 @@ void last_radio_from_ppm() { uint8_t i; for(i = 0; i < RADIO_CTL_NB; i++) { - int16_t pprz = Int16FromPulse(i); + int16_t pprz = NewInt16FromPulse(i); if (pprz > MAX_PPRZ) pprz = MAX_PPRZ; else if (pprz < MIN_PPRZ) diff --git a/sw/airborne/fly_by_wire/ppm.h b/sw/airborne/fly_by_wire/ppm.h index fab02455e1..332bbb99cb 100644 --- a/sw/airborne/fly_by_wire/ppm.h +++ b/sw/airborne/fly_by_wire/ppm.h @@ -89,6 +89,7 @@ ppm_init( void ) #define PPM_NB_PULSES RADIO_CTL_NB extern volatile bool_t ppm_valid; +extern uint16_t ppm_pulses[PPM_NB_PULSES]; extern pprz_t last_radio[PPM_NB_PULSES]; extern bool_t last_radio_contains_avg_channels; @@ -99,4 +100,28 @@ extern bool_t last_radio_contains_avg_channels; #define MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? MODE_MANUAL : MODE_AUTO) extern void last_radio_from_ppm(void); + +#define STALLED_TIME 30 // 500ms with a 60Hz timer +#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer +extern uint16_t ppm_time_since_last_valid; + +#define PPM_STATUS_OK 0 +#define PPM_STATUS_LOST 1 +#define PPM_STATUS_REALLY_LOST 2 +extern uint8_t ppm_status; +#define PPM_UPDATE_TIMER() { \ + if (ppm_time_since_last_valid >= REALLY_STALLED_TIME) \ + ppm_status = PPM_STATUS_REALLY_LOST; \ + else { \ + ppm_time_since_last_valid++; \ + if (ppm_time_since_last_valid >= STALLED_TIME) \ + ppm_status = PPM_STATUS_LOST; \ + else \ + ppm_status = PPM_STATUS_OK; \ + } \ +} + + + + #endif diff --git a/sw/airborne/fly_by_wire/servo.c b/sw/airborne/fly_by_wire/servo.c index ae7e525375..a11c81fb0c 100644 --- a/sw/airborne/fly_by_wire/servo.c +++ b/sw/airborne/fly_by_wire/servo.c @@ -38,7 +38,6 @@ * Paparazzi boards have one 4017 servo driver. * It is driven by OCR1A (PB1) with reset on PORTD5. */ -#define _4017_NB_CHANNELS 10 #ifdef CTL_BRD_V1_1 #define _4017_RESET_PORT PORTD @@ -82,7 +81,7 @@ #define ChopServo(x) ((x) < SERVO_MIN ? SERVO_MIN : ((x) > SERVO_MAX ? SERVO_MAX : (x))) /* holds the servo pulses width in clock ticks */ -static uint16_t servo_widths[_4017_NB_CHANNELS]; +uint16_t servo_widths[_4017_NB_CHANNELS]; /* * We use the output compare registers to generate our servo pulses. diff --git a/sw/airborne/fly_by_wire/servo.h b/sw/airborne/fly_by_wire/servo.h index cd15fa2baa..87022745ee 100644 --- a/sw/airborne/fly_by_wire/servo.h +++ b/sw/airborne/fly_by_wire/servo.h @@ -57,5 +57,7 @@ extern void servo_set(const pprz_t values[]); extern void servo_set_one(uint8_t servo, uint16_t value_us); extern void servo_transmit(void); +#define _4017_NB_CHANNELS 10 +extern uint16_t servo_widths[_4017_NB_CHANNELS]; #endif /* SERVO_H */ diff --git a/sw/airborne/fly_by_wire/telemetry.c b/sw/airborne/fly_by_wire/telemetry.c index 2b45e27fab..4dea07082c 100644 --- a/sw/airborne/fly_by_wire/telemetry.c +++ b/sw/airborne/fly_by_wire/telemetry.c @@ -1,14 +1,3 @@ #include "telemetry.h" -#include + uint8_t ck_a, ck_b; - -#define DL_FBW_STATUS 0x04 - -void telemetry_send_fbw_status(uint8_t* nb_spi_err, uint8_t* rc_status, uint8_t* mode) { if (TELEMETRY_CHECK_FREE_SPACE(7)) { - TELEMETRY_START_MESSAGE(DL_FBW_STATUS); - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(nb_spi_err)); - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(rc_status)); - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)(mode)); - TELEMETRY_END_MESSAGE(); - } -} diff --git a/sw/airborne/fly_by_wire/telemetry.h b/sw/airborne/fly_by_wire/telemetry.h index fc8fa3633d..b31d66f6f2 100644 --- a/sw/airborne/fly_by_wire/telemetry.h +++ b/sw/airborne/fly_by_wire/telemetry.h @@ -33,7 +33,6 @@ extern uint8_t ck_a, ck_b; #include "uart.h" -void telemetry_send_fbw_status(uint8_t* nb_spi_err, uint8_t* rc_status, uint8_t* mode); #define TELEMETRY_START_MESSAGE(id) { \ UART_PUT_1_BYTE(STX); UART_PUT_1_BYTE(id); ck_a = id; ck_b = id; \ @@ -52,15 +51,91 @@ void telemetry_send_fbw_status(uint8_t* nb_spi_err, uint8_t* rc_status, uint8_t* } #define TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(_byte) { \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte); \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+1); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte+1); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte); \ } -#define TELEMETRY_PUT_4_DATA_BYTE_BY_ADDR(_byte) { \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte); \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+1); \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+2); \ - TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(_byte+3); \ +#define TELEMETRY_PUT_4_DATA_BYTE_BY_ADDR(_byte) { \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte+3); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte+2); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte+1); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR((uint8_t*)_byte); \ } +#define DL_PPM 0x01 +#define DL_RC 0x02 +#define DL_SERVOS 0x03 +#define DL_FBW_STATUS 0x04 + +#define TELEMETRY_SEND_PPM(nb_channels, sync_len, ppm01, ppm02, ppm03,ppm04, ppm05, ppm06, ppm07, ppm08, ppm09, ppm10, ppm11, ppm12) { \ + if (TELEMETRY_CHECK_FREE_SPACE(30)) { \ + TELEMETRY_START_MESSAGE(DL_PPM); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(nb_channels); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(sync_len); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm01); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm02); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm03); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm04); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm05); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm06); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm07); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm08); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm09); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm10); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm11); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ppm12); \ + TELEMETRY_END_MESSAGE(); \ + } \ +} + +#define TELEMETRY_SEND_RC(nb_channels, pprz01, pprz02, pprz03, pprz04, pprz05, pprz06, pprz07, pprz08, pprz09, pprz10, pprz11, pprz12) { \ + if (TELEMETRY_CHECK_FREE_SPACE(29)) { \ + TELEMETRY_START_MESSAGE(DL_RC); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(nb_channels); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz01); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz02); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz03); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz04); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz05); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz06); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz07); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz08); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz09); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz10); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz11); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(pprz12); \ + TELEMETRY_END_MESSAGE(); \ + } \ +} + +#define TELEMETRY_SEND_SERVOS(nb_servos, ser01, ser02, ser03, ser04, ser05, ser06, ser07, ser08, ser09, ser10) { \ + if (TELEMETRY_CHECK_FREE_SPACE(25)) { \ + TELEMETRY_START_MESSAGE(DL_SERVOS); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(nb_servos); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser01); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser02); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser03); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser04); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser05); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser06); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser07); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser08); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser09); \ + TELEMETRY_PUT_2_DATA_BYTE_BY_ADDR(ser10); \ + TELEMETRY_END_MESSAGE(); \ + } \ +} + + +#define TELEMETRY_SEND_FBW_STATUS(nb_spi_err, rc_status, mode) { \ + if (TELEMETRY_CHECK_FREE_SPACE(7)) { \ + TELEMETRY_START_MESSAGE(DL_FBW_STATUS); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(nb_spi_err); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(rc_status); \ + TELEMETRY_PUT_1_DATA_BYTE_BY_ADDR(mode); \ + TELEMETRY_END_MESSAGE(); \ + } \ +} + + #endif /* TELEMETRY_H */ diff --git a/sw/airborne/fly_by_wire/test/Makefile b/sw/airborne/fly_by_wire/test/Makefile index a027538649..2c52e77bf3 100644 --- a/sw/airborne/fly_by_wire/test/Makefile +++ b/sw/airborne/fly_by_wire/test/Makefile @@ -33,7 +33,6 @@ TARGET = check_uart VARINCLUDE = $(PAPARAZZI_HOME)/var/include ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) INCLUDES = -I ../ -I ../../../include -I $(ACINCLUDE) -I $(VARINCLUDE) -#-I ../../../var/include CONF_DIR = ../../../../conf CONF_XML = $(CONF_DIR)/conf.xml @@ -51,9 +50,13 @@ ifeq ($(CTL_BRD_VERSION),V1_1) CTL_BRD_FLAGS=-DCTL_BRD_V1_1 endif -rc_transmitter.srcs = rc_transmitter.c ../ppm.c ../uart.c +ppm_diag.srcs = ppm_diag.c ppm_test.c ../uart.c ../telemetry.c -new_rc_transmitter.srcs = new_rc_transmitter.c ../ppm.c ../uart.c ../telemetry.c +rc_diag.srcs = rc_diag.c ../ppm.c ../uart.c ../telemetry.c + +servo_diag.srcs = servo_diag.c ../ppm.c ../uart.c ../telemetry.c ../servo.c + +rc_transmitter.srcs = rc_transmitter.c ../ppm.c ../uart.c setup_servos.srcs = setup_servos.c ../uart.c ../servo.c @@ -63,7 +66,6 @@ tx_adcs.srcs = tx_adcs.c ../uart.c ../adc_fbw.c check_spi.srcs = check_spi.c ../uart.c ../spi.c - include ../../../../conf/Makefile.local include ../../../../conf/Makefile.avr diff --git a/sw/airborne/fly_by_wire/test/new_rc_transmitter.c b/sw/airborne/fly_by_wire/test/new_rc_transmitter.c deleted file mode 100644 index 3c3abe5f8b..0000000000 --- a/sw/airborne/fly_by_wire/test/new_rc_transmitter.c +++ /dev/null @@ -1,38 +0,0 @@ - -#include -#include - -#include "timer.h" -#include "uart.h" -#include "ppm.h" - -#include "telemetry.h" - -uint8_t nb_spi_err = 0; -uint8_t rc_status = 0; -uint8_t mode = 0; - -int main( void ) { - uart_init_tx(); - timer_init(); - ppm_init(); - sei(); - int n = 0; - while( 1 ) { - if( ppm_valid ) { - ppm_valid = FALSE; - } - if (timer_periodic()) { - n++; - if (n == 60) { - n = 0; - // uart_transmit('A'); - // uart_transmit('\n'); - telemetry_send_fbw_status(&nb_spi_err, &rc_status, &mode); - } - } - } - return 0; -} - - diff --git a/sw/airborne/fly_by_wire/test/ppm_diag.c b/sw/airborne/fly_by_wire/test/ppm_diag.c new file mode 100644 index 0000000000..0bc9a20fba --- /dev/null +++ b/sw/airborne/fly_by_wire/test/ppm_diag.c @@ -0,0 +1,49 @@ + +#include +#include + +#include "timer.h" +#include "uart.h" +#include "ppm_test.h" + +#include "telemetry.h" + +uint8_t nb_spi_err = 0; +uint8_t rc_status = 0; +uint8_t mode = 0; + +int main( void ) { + uart_init_tx(); + timer_init(); + ppm_init(); + sei(); + int n = 0; + int m = 0; + while( 1 ) { + if( ppm_available ) { + ppm_mainloop_task(); + m++; + if (m == 20) { + m = 0; + TELEMETRY_SEND_PPM(&ppm_nb_received_channel, &ppm_sync_len, + &ppm_pulses[0], &ppm_pulses[1], &ppm_pulses[2], &ppm_pulses[3], + &ppm_pulses[4], &ppm_pulses[5], &ppm_pulses[6], &ppm_pulses[7], + &ppm_pulses[8], &ppm_pulses[9], &ppm_pulses[10], &ppm_pulses[11]); + } + } + + if (timer_periodic()) { /* 61 Hz */ + PPM_UPDATE_TIMER(); + n++; + if (n == 60) { + n = 0; + TELEMETRY_SEND_FBW_STATUS(&nb_spi_err, &ppm_status, &mode); + } + } + } + return 0; +} + + + + diff --git a/sw/airborne/fly_by_wire/test/ppm_test.c b/sw/airborne/fly_by_wire/test/ppm_test.c new file mode 100644 index 0000000000..51f0c04f3e --- /dev/null +++ b/sw/airborne/fly_by_wire/test/ppm_test.c @@ -0,0 +1,91 @@ +/* $Id$ + * Copied from autopilot (autopilot.sf.net) thanx alot Trammell + * + * (c) 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. + * + */ + +#include "ppm_test.h" + +/* + * Sync pulses are timed with Timer2, which runs at Clk/1024. This + * is slow enough at 16MHz to measure up to a 16ms sync pulse. + * + * Otherwise, compute the pulse width with the 16-bit timer1, + * + */ + +uint16_t ppm_pulses[ PPM_MAX_PULSES ]; +uint16_t ppm_pulses_buf[ PPM_MAX_PULSES ]; +volatile bool_t ppm_available; +uint16_t ppm_time_since_last_valid; +uint8_t ppm_status; +uint8_t ppm_nb_received_channel; +uint8_t ppm_sync_len; + +SIGNAL( SIG_INPUT_CAPTURE1 ) +{ + /* t1 is running @ CLOCK */ + static uint16_t last_t1; + uint16_t cur_t1; + uint16_t data_len; + /* t2 is running @ CLOCK/1024 */ + static uint8_t last_t2; + uint8_t cur_t2; + uint8_t sync_len; + + static uint8_t data_pulse_idx; + + /* compute elapsed timer1 ticks */ + /* allows high resolution on data pulses */ + cur_t1 = ICR1; + data_len = cur_t1 - last_t1; + last_t1 = cur_t1; + + /* compute elapsed timer2 ticks */ + /* allows measuring long sync pulse */ + cur_t2 = TCNT2; + sync_len = cur_t2 - last_t2; + last_t2 = cur_t2; + + /* we have a pulse that is >= 3ms ; Let say it's a sync pulse*/ + if (sync_len >= (uint8_t)(((uint32_t)(3000ul*CLOCK))/1024ul)) { + ppm_sync_len = sync_len; /* store the sync pulse len */ + ppm_nb_received_channel = data_pulse_idx; /* store how many channels we received before */ + if (ppm_nb_received_channel != 0) + ppm_available = TRUE; + data_pulse_idx = 0; /* prepare to receive a new frame */ + } + /* let's assume we have a data pulse */ + else { + if (data_pulse_idx <= PPM_MAX_PULSES) { + ppm_pulses_buf[data_pulse_idx] = data_len; + data_pulse_idx++; + } + } +} + +void ppm_mainloop_task(void) { + uint8_t i; + ppm_available = FALSE; + for (i=0; i +#include + +#include "timer.h" +#include "link_autopilot.h" + +#define PpmOfUs(x) ((x)*CLOCK) + +#define PPM_DDR DDRB +#define PPM_PORT PORTB +#define PPM_PIN PB0 + +/* + * PPM pulses are falling edge clocked on the ICP, which records + * the state of the global clock. + * + * JR might be rising edge clocked; set that as an option + */ + + +#define PPM_MAX_PULSES 12 +extern volatile bool_t ppm_available; +extern uint16_t ppm_pulses[PPM_MAX_PULSES]; +extern uint8_t ppm_nb_received_channel; +extern uint8_t ppm_sync_len; + +#define STALLED_TIME 30 // 500ms with a 60Hz timer +#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer +extern uint16_t ppm_time_since_last_valid; + +#define PPM_STATUS_OK 0 +#define PPM_STATUS_LOST 1 +#define PPM_STATUS_REALLY_LOST 2 +extern uint8_t ppm_status; + +static inline void +ppm_init( void ) +{ + uint8_t i; +#if PPM_RX_TYPE == RXFUTABA + cbi( TCCR1B, ICES1 ); +#elif PPM_RX_TYPE == RXJR + sbi( TCCR1B, ICES1 ); +#else +# error "ppm.h: Unknown receiver type in PPM_RX_TYPE" +#endif + + /* No noise cancelation */ + sbi( TCCR1B, ICNC1 ); + + /* Set ICP to input */ + cbi( PPM_DDR, PPM_PIN); + /* no internal pull up */ + cbi( PPM_PORT, PPM_PIN); + + /* Enable interrupt on input capture */ + sbi( TIMSK, TICIE1 ); + + for (i=0; i= REALLY_STALLED_TIME) \ + ppm_status = PPM_STATUS_REALLY_LOST; \ + else { \ + ppm_time_since_last_valid++; \ + if (ppm_time_since_last_valid >= STALLED_TIME) \ + ppm_status = PPM_STATUS_LOST; \ + else \ + ppm_status = PPM_STATUS_OK; \ + } \ +} + +extern void ppm_mainloop_task(void); + +#endif /* PPM_TEST_H */ diff --git a/sw/airborne/fly_by_wire/test/rc_diag.c b/sw/airborne/fly_by_wire/test/rc_diag.c new file mode 100644 index 0000000000..8d2b4c7cbd --- /dev/null +++ b/sw/airborne/fly_by_wire/test/rc_diag.c @@ -0,0 +1,59 @@ + +#include +#include + +#include "timer.h" +#include "uart.h" +#include "ppm.h" +#include "telemetry.h" + +uint8_t nb_spi_err = 0; +uint8_t rc_status = 0; +uint8_t mode = 0; + +uint8_t nb_channels = RADIO_CTL_NB; + +uint8_t ppm_sync_len = 0; +uint8_t ppm_nb_received_channel = 0; + +int main( void ) { + uart_init_tx(); + timer_init(); + ppm_init(); + sei(); + int n = 0; + int m = 0; + while( 1 ) { + if( ppm_valid ) { + ppm_valid = FALSE; + ppm_time_since_last_valid = 0; + last_radio_from_ppm(); + m++; + if (m == 20) { + m = 0; + TELEMETRY_SEND_RC(&nb_channels, + &last_radio[0], &last_radio[1], &last_radio[2], &last_radio[3], + &last_radio[4], &last_radio[5], &last_radio[6], &last_radio[7], + &last_radio[8], &last_radio[9], &last_radio[10], &last_radio[11]); + TELEMETRY_SEND_PPM(&ppm_nb_received_channel, &ppm_sync_len, + &ppm_pulses[0], &ppm_pulses[1], &ppm_pulses[2], &ppm_pulses[3], + &ppm_pulses[4], &ppm_pulses[5], &ppm_pulses[6], &ppm_pulses[7], + &ppm_pulses[8], &ppm_pulses[9], &ppm_pulses[10], &ppm_pulses[11]); + } + } + + if (timer_periodic()) { /* 61 Hz */ + PPM_UPDATE_TIMER(); + n++; + if (n == 60) { + n = 0; + TELEMETRY_SEND_FBW_STATUS(&nb_spi_err, &ppm_status, &mode); + } + } + } + return 0; +} + + + + diff --git a/sw/airborne/fly_by_wire/test/servo_diag.c b/sw/airborne/fly_by_wire/test/servo_diag.c new file mode 100644 index 0000000000..0d2d60a402 --- /dev/null +++ b/sw/airborne/fly_by_wire/test/servo_diag.c @@ -0,0 +1,72 @@ + +#include +#include + +#include "timer.h" +#include "ppm.h" +#include "telemetry.h" +#include "servo.h" + +uint8_t nb_spi_err = 0; +uint8_t rc_status = 0; +uint8_t mode = 0; + +uint8_t nb_channels = RADIO_CTL_NB; +uint8_t nb_servos = _4017_NB_CHANNELS; + +uint8_t ppm_sync_len = 0; +uint8_t ppm_nb_received_channel = 0; + +int main( void ) { + uart_init_tx(); + ppm_init(); + servo_init(); + timer_init(); + sei(); + int n = 0; + int m = 0; + while( 1 ) { + if( ppm_valid ) { + ppm_valid = FALSE; + ppm_time_since_last_valid = 0; + last_radio_from_ppm(); + servo_set(last_radio); + m++; + if (m == 20) { + m = 0; + TELEMETRY_SEND_RC(&nb_channels, + &last_radio[0], &last_radio[1], &last_radio[2], &last_radio[3], + &last_radio[4], &last_radio[5], &last_radio[6], &last_radio[7], + &last_radio[8], &last_radio[9], &last_radio[10], &last_radio[11]); +/* TELEMETRY_SEND_PPM(&ppm_nb_received_channel, &ppm_sync_len, */ +/* &ppm_pulses[0], &ppm_pulses[1], &ppm_pulses[2], &ppm_pulses[3], */ +/* &ppm_pulses[4], &ppm_pulses[5], &ppm_pulses[6], &ppm_pulses[7], */ +/* &ppm_pulses[8], &ppm_pulses[9], &ppm_pulses[10], &ppm_pulses[11]); */ + } + } + + if (timer_periodic()) { /* 61 Hz */ + PPM_UPDATE_TIMER(); + n++; + if (n == 60) { + n = 0; + TELEMETRY_SEND_FBW_STATUS(&nb_spi_err, &ppm_status, &mode); + { + static uint16_t servo_us[_4017_NB_CHANNELS]; + uint8_t i; + for (i=0; i<_4017_NB_CHANNELS; i++) + servo_us[i] = servo_widths[i] / CLOCK; + TELEMETRY_SEND_SERVOS(&nb_servos, + &servo_us[0], &servo_us[1], &servo_us[2], &servo_us[3], + &servo_us[4], &servo_us[5], &servo_us[6], &servo_us[7], + &servo_us[8], &servo_us[9]); + } + } + } + } + return 0; +} + + + +