Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

some more diag update

Browse Source
This commit is contained in:
Antoine Drouin
2005-02-27 04:26:45 +00:00
parent 689f58aa28
commit b70035be8b
14 changed files with 521 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fly_by_wire/main.c
+1 -1
View File
@@ -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]);
sw/airborne/fly_by_wire/ppm.c
+7 -4
View File
@@ -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)
sw/airborne/fly_by_wire/ppm.h
+25
View File
@@ -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
sw/airborne/fly_by_wire/servo.c
+1 -2
View File
@@ -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.
sw/airborne/fly_by_wire/servo.h
+2
View File
@@ -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 */
sw/airborne/fly_by_wire/telemetry.c
+1 -12
View File
@@ -1,14 +1,3 @@
#include "telemetry.h"
#include <avr/io.h>
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();
}
}
sw/airborne/fly_by_wire/telemetry.h
+83 -8
View File
@@ -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 */
sw/airborne/fly_by_wire/test/Makefile
+6 -4
View File
@@ -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
sw/airborne/fly_by_wire/test/new_rc_transmitter.c
-38
View File
@@ -1,38 +0,0 @@
#include <inttypes.h>
#include <avr/interrupt.h>
#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;
}
sw/airborne/fly_by_wire/test/ppm_diag.c
+49
View File
@@ -0,0 +1,49 @@
#include <inttypes.h>
#include <avr/interrupt.h>
#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;
}
sw/airborne/fly_by_wire/test/ppm_test.c
+91
View File
@@ -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<ppm_nb_received_channel; i++)
ppm_pulses[i] = ppm_pulses_buf[i]/CLOCK;
}
sw/airborne/fly_by_wire/test/ppm_test.h
+124
View File
@@ -0,0 +1,124 @@
/* $Id$
*
* Decoder for the trainer ports or hacked receivers for both
* Futaba and JR formats. The ppm_valid flag is set whenever
* a valid frame is received.
*
* Pulse widths are stored as unscaled 16-bit values in ppm_pulses[].
* If you require actual microsecond values, divide by CLOCK.
* For an 8 Mhz clock and typical servo values, these will range
* from 0x1F00 to 0x4000.
*
* 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.
*
*/
#ifndef PPM_TEST_H
#define PPM_TEST_H
/**
* Receiver types
*/
#define RXFUTABA 0
#define RXJR 1
#define PPM_RX_TYPE RXFUTABA
#define PPM_FREQ 40 // 25ms
#include <inttypes.h>
#include <avr/signal.h>
#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<PPM_MAX_PULSES; i++)
ppm_pulses[i] = 1550+i;
}
#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; \
} \
}
extern void ppm_mainloop_task(void);
#endif /* PPM_TEST_H */
sw/airborne/fly_by_wire/test/rc_diag.c
+59
View File
@@ -0,0 +1,59 @@
#include <inttypes.h>
#include <avr/interrupt.h>
#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;
}
sw/airborne/fly_by_wire/test/servo_diag.c
+72
View File
@@ -0,0 +1,72 @@
#include <inttypes.h>
#include <avr/interrupt.h>
#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;
}