mirror of
https://github.com/opentx/opentx.git
synced 2025-07-26 09:45:21 +03:00
Telemetry Screens again!
This commit is contained in:
parent
2b5eb64540
commit
695ecf7a0d
18 changed files with 818 additions and 143 deletions
20
src/Makefile
20
src/Makefile
|
@ -28,7 +28,7 @@
|
||||||
#----------- BUILD OPTIONS ---------------------------
|
#----------- BUILD OPTIONS ---------------------------
|
||||||
|
|
||||||
#gruvin: PCB version -- OVERRIDES the following settings if not STD
|
#gruvin: PCB version -- OVERRIDES the following settings if not STD
|
||||||
# Values: STD, V4
|
# Values: STD, V4, ARM
|
||||||
PCB = STD
|
PCB = STD
|
||||||
|
|
||||||
# Following options for PCB=STD only (ignored otherwise) ...
|
# Following options for PCB=STD only (ignored otherwise) ...
|
||||||
|
@ -130,13 +130,17 @@ AREV = $(shell sh -c "cat .svn/entries | sed -n '4p'")
|
||||||
REV = $(shell echo $$(( $(AREV) + 1 )))
|
REV = $(shell echo $$(( $(AREV) + 1 )))
|
||||||
|
|
||||||
# MCU name
|
# MCU name
|
||||||
|
ifeq ($(PCB), ARM)
|
||||||
|
MCU = cortex-m3
|
||||||
|
BOARDSRC = board_arm.cpp
|
||||||
|
endif
|
||||||
ifeq ($(PCB), STD)
|
ifeq ($(PCB), STD)
|
||||||
MCU = atmega64
|
MCU = atmega64
|
||||||
CPPSRC = stockboard.cpp
|
BOARDSRC = board_stock.cpp
|
||||||
endif
|
endif
|
||||||
ifeq ($(PCB), V4)
|
ifeq ($(PCB), V4)
|
||||||
MCU = atmega2560
|
MCU = atmega2560
|
||||||
CPPSRC = v4board.cpp
|
BOARDSRC = board_gruvin9x.cpp
|
||||||
endif
|
endif
|
||||||
|
|
||||||
# Processor frequency.
|
# Processor frequency.
|
||||||
|
@ -152,7 +156,7 @@ TARGET = open9x
|
||||||
OBJDIR = obj
|
OBJDIR = obj
|
||||||
|
|
||||||
# List C++ source files here. (C dependencies are automatically generated.)
|
# List C++ source files here. (C dependencies are automatically generated.)
|
||||||
CPPSRC += open9x.cpp pulses.cpp stamp.cpp menus.cpp model_menus.cpp general_menus.cpp main_views.cpp statistics_views.cpp pers.cpp file.cpp lcd.cpp drivers.cpp o9xstrings.cpp
|
CPPSRC = open9x.cpp pulses.cpp stamp.cpp menus.cpp model_menus.cpp general_menus.cpp main_views.cpp statistics_views.cpp pers.cpp file.cpp lcd.cpp drivers.cpp o9xstrings.cpp
|
||||||
|
|
||||||
ifeq ($(EXT), JETI)
|
ifeq ($(EXT), JETI)
|
||||||
CPPSRC += jeti.cpp
|
CPPSRC += jeti.cpp
|
||||||
|
@ -746,9 +750,9 @@ extcoff: $(TARGET).elf
|
||||||
$(NM) -n $< > $@
|
$(NM) -n $< > $@
|
||||||
|
|
||||||
# Concatenate all sources files in one big file to optimize size
|
# Concatenate all sources files in one big file to optimize size
|
||||||
allsrc: $(CPPSRC)
|
allsrc: $(BOARDSRC) $(CPPSRC)
|
||||||
@echo -n > allsrc.cpp
|
@echo -n > allsrc.cpp
|
||||||
for f in $(CPPSRC); do echo "# 1 \"$$f\"" >> allsrc.cpp; cat "$$f" >> allsrc.cpp; done
|
for f in $(BOARDSRC) $(CPPSRC); do echo "# 1 \"$$f\"" >> allsrc.cpp; cat "$$f" >> allsrc.cpp; done
|
||||||
|
|
||||||
remallsrc:
|
remallsrc:
|
||||||
$(REMOVE) allsrc.cpp
|
$(REMOVE) allsrc.cpp
|
||||||
|
|
654
src/board_ersky9x.cpp
Normal file
654
src/board_ersky9x.cpp
Normal file
|
@ -0,0 +1,654 @@
|
||||||
|
/*
|
||||||
|
* Authors (alphabetical order)
|
||||||
|
* - Bertrand Songis <bsongis@gmail.com>
|
||||||
|
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
|
||||||
|
* - Cameron Weeks <th9xer@gmail.com>
|
||||||
|
* - Erez Raviv
|
||||||
|
* - Jean-Pierre Parisy
|
||||||
|
* - Karl Szmutny <shadow@privy.de>
|
||||||
|
* - Michael Blandford
|
||||||
|
* - Michal Hlavinka
|
||||||
|
* - Pat Mackenzie
|
||||||
|
* - Philip Moss
|
||||||
|
* - Rob Thomson
|
||||||
|
* - Romolo Manfredini <romolo.manfredini@gmail.com>
|
||||||
|
* - Thomas Husterer
|
||||||
|
*
|
||||||
|
* open9x is based on code named
|
||||||
|
* gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
|
||||||
|
* er9x by Erez Raviv: http://code.google.com/p/er9x/,
|
||||||
|
* and the original (and ongoing) project by
|
||||||
|
* Thomas Husterer, th9x: http://code.google.com/p/th9x/
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License version 2 as
|
||||||
|
* published by the Free Software Foundation.
|
||||||
|
*
|
||||||
|
* This program 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.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "open9x.h"
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
inline void init_soft_power()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
// Configure RF_power (PC17) but not PPM-jack-in (PC22), neither need pullups
|
||||||
|
pioptr->PIO_PER = PIO_PC17 ; // Enable bit C17
|
||||||
|
pioptr->PIO_ODR = PIO_PC17 ; // Set bit C17 as input
|
||||||
|
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_PER = PIO_PA8 ; // Enable bit A8 (Soft Power)
|
||||||
|
pioptr->PIO_ODR = PIO_PA8 ; // Set bit A8 as input
|
||||||
|
pioptr->PIO_PUER = PIO_PA8 ; // Enable PA8 pullup
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Prototype
|
||||||
|
// Free pins (PA16 is stock buzzer)
|
||||||
|
// PA23, PA24, PA25, PB7, PB13
|
||||||
|
// PC20, PC21(labelled 17), PC22, PC24
|
||||||
|
// REVB
|
||||||
|
// PA25, use for stock buzzer
|
||||||
|
// PB14, PB6
|
||||||
|
// PC21, PC19, PC15 (PPM2 output)
|
||||||
|
inline void config_free_pins()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr = PIOB ;
|
||||||
|
pioptr->PIO_PER = 0x00004040L ; // Enable bits B14, 6
|
||||||
|
pioptr->PIO_ODR = 0x00004040L ; // Set as input
|
||||||
|
pioptr->PIO_PUER = 0x00004040L ; // Enable pullups
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = 0x00280000L ; // Enable bits C21, 19
|
||||||
|
pioptr->PIO_ODR = 0x00280000L ; // Set as input
|
||||||
|
pioptr->PIO_PUER = 0x00280000L ; // Enable pullups
|
||||||
|
#else
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_PER = 0x03800000L ; // Enable bits A25,24,23
|
||||||
|
pioptr->PIO_ODR = 0x03800000L ; // Set as input
|
||||||
|
pioptr->PIO_PUER = 0x03800000L ; // Enable pullups
|
||||||
|
|
||||||
|
pioptr = PIOB ;
|
||||||
|
pioptr->PIO_PER = 0x00002080L ; // Enable bits B13, 7
|
||||||
|
pioptr->PIO_ODR = 0x00002080L ; // Set as input
|
||||||
|
pioptr->PIO_PUER = 0x00002080L ; // Enable pullups
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = 0x01700000L ; // Enable bits C24,22,21,20
|
||||||
|
pioptr->PIO_ODR = 0x01700000L ; // Set as input
|
||||||
|
pioptr->PIO_PUER = 0x01700000L ; // Enable pullups
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// Assumes PMC has already enabled clocks to ports
|
||||||
|
inline void setup_switches()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
pioptr = PIOA ;
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_PER = 0x01808087 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0x01808087 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0x01808087 ; // Set bits with pullups
|
||||||
|
#else
|
||||||
|
pioptr->PIO_PER = 0xF8008184 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0xF8008184 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0xF8008184 ; // Set bits with pullups
|
||||||
|
#endif
|
||||||
|
pioptr = PIOB ;
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_PER = 0x00000030 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0x00000030 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0x00000030 ; // Set bits with pullups
|
||||||
|
#else
|
||||||
|
pioptr->PIO_PER = 0x00000010 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0x00000010 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0x00000010 ; // Set bits with pullups
|
||||||
|
#endif
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_PER = 0x91114900 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0x91114900 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0x91114900 ; // Set bits with pullups
|
||||||
|
#else
|
||||||
|
pioptr->PIO_PER = 0x10014900 ; // Enable bits
|
||||||
|
pioptr->PIO_ODR = 0x10014900 ; // Set bits input
|
||||||
|
pioptr->PIO_PUER = 0x10014900 ; // Set bits with pullups
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Configures a UART peripheral with the specified parameters.
|
||||||
|
*
|
||||||
|
* baudrate Baudrate at which the UART should operate (in Hz).
|
||||||
|
* masterClock Frequency of the system master clock (in Hz).
|
||||||
|
* uses PA9 and PA10, RXD2 and TXD2
|
||||||
|
*/
|
||||||
|
inline void UART_Configure( uint32_t baudrate, uint32_t masterClock)
|
||||||
|
{
|
||||||
|
// const Pin pPins[] = CONSOLE_PINS;
|
||||||
|
register Uart *pUart = CONSOLE_USART;
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
/* Configure PIO */
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_ABCDSR[0] &= ~0x00000600 ; // Peripheral A
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~0x00000600 ; // Peripheral A
|
||||||
|
pioptr->PIO_PDR = 0x00000600 ; // Assign to peripheral
|
||||||
|
|
||||||
|
/* Configure PMC */
|
||||||
|
PMC->PMC_PCER0 = 1 << CONSOLE_ID;
|
||||||
|
|
||||||
|
/* Reset and disable receiver & transmitter */
|
||||||
|
pUart->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX
|
||||||
|
| UART_CR_RXDIS | UART_CR_TXDIS;
|
||||||
|
|
||||||
|
/* Configure mode */
|
||||||
|
pUart->UART_MR = 0x800 ; // NORMAL, No Parity
|
||||||
|
|
||||||
|
/* Configure baudrate */
|
||||||
|
/* Asynchronous, no oversampling */
|
||||||
|
pUart->UART_BRGR = (masterClock / baudrate) / 16;
|
||||||
|
|
||||||
|
/* Disable PDC channel */
|
||||||
|
pUart->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
|
||||||
|
|
||||||
|
/* Enable receiver and transmitter */
|
||||||
|
pUart->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void UART3_Configure( uint32_t baudrate, uint32_t masterClock)
|
||||||
|
{
|
||||||
|
// const Pin pPins[] = CONSOLE_PINS;
|
||||||
|
register Uart *pUart = BT_USART;
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
/* Configure PIO */
|
||||||
|
pioptr = PIOB ;
|
||||||
|
pioptr->PIO_ABCDSR[0] &= ~(PIO_PB2 | PIO_PB3) ; // Peripheral A
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~(PIO_PB2 | PIO_PB3) ; // Peripheral A
|
||||||
|
pioptr->PIO_PDR = (PIO_PB2 | PIO_PB3) ; // Assign to peripheral
|
||||||
|
|
||||||
|
/* Configure PMC */
|
||||||
|
PMC->PMC_PCER0 = 1 << BT_ID;
|
||||||
|
|
||||||
|
/* Reset and disable receiver & transmitter */
|
||||||
|
pUart->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX
|
||||||
|
| UART_CR_RXDIS | UART_CR_TXDIS;
|
||||||
|
|
||||||
|
/* Configure mode */
|
||||||
|
pUart->UART_MR = 0x800 ; // NORMAL, No Parity
|
||||||
|
|
||||||
|
/* Configure baudrate */
|
||||||
|
/* Asynchronous, no oversampling */
|
||||||
|
pUart->UART_BRGR = (masterClock / baudrate) / 16;
|
||||||
|
|
||||||
|
// baudrate = (masterClock * 8 / baudrate) / 16 ;
|
||||||
|
// pUart->UART_BRGR = ( baudrate / 8 ) || ( ( baudrate & 7 ) << 16 ) ; // Fractional part to allow 152000 baud
|
||||||
|
//
|
||||||
|
/* Disable PDC channel */
|
||||||
|
pUart->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
|
||||||
|
|
||||||
|
/* Enable receiver and transmitter */
|
||||||
|
pUart->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
|
||||||
|
}
|
||||||
|
|
||||||
|
// USART0 configuration
|
||||||
|
// Work in Progress, UNTESTED
|
||||||
|
// Uses PA5 and PA6 (RXD and TXD)
|
||||||
|
inline void UART2_Configure( uint32_t baudrate, uint32_t masterClock)
|
||||||
|
{
|
||||||
|
//// const Pin pPins[] = CONSOLE_PINS;
|
||||||
|
register Usart *pUsart = SECOND_USART;
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
/* Configure PIO */
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_ABCDSR[0] &= ~0x00000060 ; // Peripheral A
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~0x00000060 ; // Peripheral A
|
||||||
|
pioptr->PIO_PDR = 0x00000060 ; // Assign to peripheral
|
||||||
|
|
||||||
|
// /* Configure PMC */
|
||||||
|
PMC->PMC_PCER0 = 1 << SECOND_ID;
|
||||||
|
|
||||||
|
// /* Reset and disable receiver & transmitter */
|
||||||
|
pUsart->US_CR = US_CR_RSTRX | US_CR_RSTTX
|
||||||
|
| US_CR_RXDIS | US_CR_TXDIS;
|
||||||
|
|
||||||
|
// /* Configure mode */
|
||||||
|
pUsart->US_MR = 0x000008C0 ; // NORMAL, No Parity
|
||||||
|
|
||||||
|
// /* Configure baudrate */
|
||||||
|
// /* Asynchronous, no oversampling */
|
||||||
|
pUsart->US_BRGR = (masterClock / baudrate) / 16;
|
||||||
|
|
||||||
|
// /* Disable PDC channel */
|
||||||
|
pUsart->US_PTCR = US_PTCR_RXTDIS | US_PTCR_TXTDIS;
|
||||||
|
|
||||||
|
// /* Enable receiver and transmitter */
|
||||||
|
pUsart->US_CR = US_CR_RXEN | US_CR_TXEN;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Starts TIMER2 at 100Hz, commentd out drive of TIOA2 (A26, EXT2) out
|
||||||
|
inline void start_timer2()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
register Tc *ptc ;
|
||||||
|
register uint32_t timer ;
|
||||||
|
|
||||||
|
// Enable peripheral clock TC0 = bit 23 thru TC5 = bit 28
|
||||||
|
PMC->PMC_PCER0 |= 0x02000000L ; // Enable peripheral clock to TC2
|
||||||
|
|
||||||
|
timer = Master_frequency / 12800 ; // MCK/128 and 100 Hz
|
||||||
|
|
||||||
|
ptc = TC0 ; // Tc block 0 (TC0-2)
|
||||||
|
ptc->TC_BCR = 0 ; // No sync
|
||||||
|
ptc->TC_BMR = 0 ;
|
||||||
|
ptc->TC_CHANNEL[2].TC_CMR = 0x00008000 ; // Waveform mode
|
||||||
|
ptc->TC_CHANNEL[2].TC_RC = timer ; // 10 Hz
|
||||||
|
ptc->TC_CHANNEL[2].TC_RA = timer >> 1 ;
|
||||||
|
ptc->TC_CHANNEL[2].TC_CMR = 0x0009C003 ; // 0000 0000 0000 1001 1100 0000 0000 0011
|
||||||
|
// MCK/128, set @ RA, Clear @ RC waveform
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = 0x00080000L ; // Enable bits C19
|
||||||
|
pioptr->PIO_OER = 0x00080000L ; // Set as output
|
||||||
|
ptc->TC_CHANNEL[2].TC_CCR = 5 ; // Enable clock and trigger it (may only need trigger)
|
||||||
|
|
||||||
|
NVIC_EnableIRQ(TC2_IRQn) ;
|
||||||
|
TC0->TC_CHANNEL[2].TC_IER = TC_IER0_CPCS ;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Test, starts TIMER0 at full speed (MCK/2) for delay timing
|
||||||
|
inline void start_timer0()
|
||||||
|
{
|
||||||
|
register Tc *ptc ;
|
||||||
|
|
||||||
|
// Enable peripheral clock TC0 = bit 23 thru TC5 = bit 28
|
||||||
|
PMC->PMC_PCER0 |= 0x00800000L ; // Enable peripheral clock to TC0
|
||||||
|
|
||||||
|
ptc = TC0 ; // Tc block 0 (TC0-2)
|
||||||
|
ptc->TC_BCR = 0 ; // No sync
|
||||||
|
ptc->TC_BMR = 2 ;
|
||||||
|
ptc->TC_CHANNEL[0].TC_CMR = 0x00008000 ; // Waveform mode
|
||||||
|
ptc->TC_CHANNEL[0].TC_RC = 0xFFF0 ;
|
||||||
|
ptc->TC_CHANNEL[0].TC_RA = 0 ;
|
||||||
|
ptc->TC_CHANNEL[0].TC_CMR = 0x00008040 ; // 0000 0000 0000 0000 1000 0000 0100 0000, stop at regC
|
||||||
|
ptc->TC_CHANNEL[0].TC_CCR = 5 ; // Enable clock and trigger it (may only need trigger)
|
||||||
|
}
|
||||||
|
|
||||||
|
// Settings for mode register ADC_MR
|
||||||
|
// USEQ off - silicon problem, doesn't work
|
||||||
|
// TRANSFER = 1
|
||||||
|
// TRACKTIM = 4 (5 clock periods)
|
||||||
|
// ANACH = 0
|
||||||
|
// SETTLING = 1 (not used if ANACH = 0)
|
||||||
|
// STARTUP = 1 (8 clock periods)
|
||||||
|
// PRESCAL = 3.6 MHz clock (between 1 and 20MHz)
|
||||||
|
// FREERUN = 0
|
||||||
|
// FWUP = 0
|
||||||
|
// SLEEP = 0
|
||||||
|
// LOWRES = 0
|
||||||
|
// TRGSEL = 0
|
||||||
|
// TRGEN = 0 (software trigger only)
|
||||||
|
inline void init_adc()
|
||||||
|
{
|
||||||
|
register Adc *padc ;
|
||||||
|
register uint32_t timer ;
|
||||||
|
|
||||||
|
timer = ( Master_frequency / (3600000*2) ) << 8 ;
|
||||||
|
// Enable peripheral clock ADC = bit 29
|
||||||
|
PMC->PMC_PCER0 |= 0x20000000L ; // Enable peripheral clock to ADC
|
||||||
|
padc = ADC ;
|
||||||
|
padc->ADC_MR = 0x14110000 | timer ; // 0001 0100 0001 0001 xxxx xxxx 0000 0000
|
||||||
|
#ifdef REVB
|
||||||
|
padc->ADC_CHER = 0x0000633E ; // channels 1,2,3,4,5,8,9,13,14
|
||||||
|
#else
|
||||||
|
padc->ADC_CHER = 0x0000623E ; // channels 1,2,3,4,5,9,13,14
|
||||||
|
#endif
|
||||||
|
padc->ADC_CGR = 0 ; // Gain = 1, all channels
|
||||||
|
padc->ADC_COR = 0 ; // Single ended, 0 offset, all channels
|
||||||
|
}
|
||||||
|
|
||||||
|
// PWM used for PPM generation, and LED Backlight
|
||||||
|
// Output pin PB5 not used, PA17 used as PWMH3 peripheral C
|
||||||
|
// PWM peripheral ID = 31 (0x80000000)
|
||||||
|
// Ensure PB5 is three state/input, used on REVB for MENU KEY
|
||||||
|
|
||||||
|
// Configure PWM3 as PPM drive,
|
||||||
|
// PWM0 is LED backlight PWM on PWMH0
|
||||||
|
// This is PC18 peripheral B, Also enable PC22 peripheral B, this is PPM-JACK (PWML3)
|
||||||
|
//
|
||||||
|
// REVB board:
|
||||||
|
// PWML2, output as peripheral C on PA16, is for HAPTIC
|
||||||
|
// For testing, just drive it out with PWM
|
||||||
|
// PWML1 for PPM2 output as peripheral B on PC15
|
||||||
|
// For testing, just drive it out with PWM
|
||||||
|
void init_pwm()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
register Pwm *pwmptr ;
|
||||||
|
register uint32_t timer ;
|
||||||
|
|
||||||
|
PMC->PMC_PCER0 |= ( 1 << ID_PWM ) ; // Enable peripheral clock to PWM
|
||||||
|
|
||||||
|
MATRIX->CCFG_SYSIO |= 0x00000020L ; // Disable TDO let PB5 work!
|
||||||
|
|
||||||
|
/* Configure PIO */
|
||||||
|
#ifndef REVB
|
||||||
|
pioptr = PIOB ;
|
||||||
|
pioptr->PIO_PER = 0x00000020L ; // Enable bit B5
|
||||||
|
pioptr->PIO_ODR = 0x00000020L ; // set as input
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr = PIOB ;
|
||||||
|
pioptr->PIO_ABCDSR[0] &= ~PIO_PA16 ; // Peripheral C
|
||||||
|
pioptr->PIO_ABCDSR[1] |= PIO_PA16 ; // Peripheral C
|
||||||
|
pioptr->PIO_PDR = PIO_PA16 ; // Disable bit A16 Assign to peripheral
|
||||||
|
#endif
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_ABCDSR[0] |= PIO_PC18 ; // Peripheral B
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~PIO_PC18 ; // Peripheral B
|
||||||
|
pioptr->PIO_PDR = PIO_PC18 ; // Disable bit C18 Assign to peripheral
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_ABCDSR[0] |= PIO_PC15 ; // Peripheral B
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~PIO_PC15 ; // Peripheral B
|
||||||
|
pioptr->PIO_PDR = PIO_PC15 ; // Disable bit C15 Assign to peripheral
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_ABCDSR[0] |= PIO_PC22 ; // Peripheral B
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~PIO_PC22 ; // Peripheral B
|
||||||
|
pioptr->PIO_PDR = PIO_PC22 ; // Disable bit C22 Assign to peripheral
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Configure clock - depends on MCK frequency
|
||||||
|
timer = Master_frequency / 2000000 ;
|
||||||
|
timer |= ( Master_frequency / ( 32* 10000 ) ) << 16 ;
|
||||||
|
timer &= 0x00FF00FF ;
|
||||||
|
|
||||||
|
pwmptr = PWM ;
|
||||||
|
pwmptr->PWM_CLK = 0x05000000 | timer ; // MCK for DIVA, DIVA = 18 gives 0.5uS clock period @35MHz // MCK/32 / timer = 10000Hz for CLKB
|
||||||
|
|
||||||
|
// PWM0 for LED backlight
|
||||||
|
pwmptr->PWM_CH_NUM[0].PWM_CMR = 0x0000000C ; // CLKB
|
||||||
|
pwmptr->PWM_CH_NUM[0].PWM_CPDR = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[0].PWM_CPDRUPD = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[0].PWM_CDTY = 40 ; // Duty
|
||||||
|
pwmptr->PWM_CH_NUM[0].PWM_CDTYUPD = 40 ; // Duty
|
||||||
|
pwmptr->PWM_ENA = PWM_ENA_CHID0 ; // Enable channel 0
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
// PWM1 for PPM2 output 100Hz test
|
||||||
|
pwmptr->PWM_CH_NUM[1].PWM_CMR = 0x0000000C ; // CLKB
|
||||||
|
pwmptr->PWM_CH_NUM[1].PWM_CPDR = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[1].PWM_CPDRUPD = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[1].PWM_CDTY = 40 ; // Duty
|
||||||
|
pwmptr->PWM_CH_NUM[1].PWM_CDTYUPD = 40 ; // Duty
|
||||||
|
pwmptr->PWM_ENA = PWM_ENA_CHID1 ; // Enable channel 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
// PWM2 for HAPTIC drive 100Hz test
|
||||||
|
pwmptr->PWM_CH_NUM[2].PWM_CMR = 0x0000000C ; // CLKB
|
||||||
|
pwmptr->PWM_CH_NUM[2].PWM_CPDR = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[2].PWM_CPDRUPD = 100 ; // Period
|
||||||
|
pwmptr->PWM_CH_NUM[2].PWM_CDTY = 40 ; // Duty
|
||||||
|
pwmptr->PWM_CH_NUM[2].PWM_CDTYUPD = 40 ; // Duty
|
||||||
|
pwmptr->PWM_ENA = PWM_ENA_CHID2 ; // Enable channel 2
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void start_sound()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
|
||||||
|
start_timer1() ;
|
||||||
|
init_dac() ;
|
||||||
|
init_twi() ;
|
||||||
|
|
||||||
|
pioptr = PIOA ;
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_CODR = 0x02000000L ; // Set bit A25 OFF
|
||||||
|
pioptr->PIO_PER = 0x02000000L ; // Enable bit A25 (Stock buzzer)
|
||||||
|
pioptr->PIO_OER = 0x02000000L ; // Set bit A25 as output
|
||||||
|
#else
|
||||||
|
pioptr->PIO_CODR = 0x00010000L ; // Set bit A16 OFF
|
||||||
|
pioptr->PIO_PER = 0x00010000L ; // Enable bit A16 (Stock buzzer)
|
||||||
|
pioptr->PIO_OER = 0x00010000L ; // Set bit A16 as output
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// SPI i/f to EEPROM (4Mb)
|
||||||
|
// Peripheral ID 21 (0x00200000)
|
||||||
|
// Connections:
|
||||||
|
// SS PA11 (peripheral A)
|
||||||
|
// MISO PA12 (peripheral A)
|
||||||
|
// MOSI PA13 (peripheral A)
|
||||||
|
// SCK PA14 (peripheral A)
|
||||||
|
// Set clock to 3 MHz, AT25 device is rated to 70MHz, 18MHz would be better
|
||||||
|
inline void init_spi()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
register Spi *spiptr ;
|
||||||
|
register uint32_t timer ;
|
||||||
|
register uint8_t *p ;
|
||||||
|
uint8_t spi_buf[4] ;
|
||||||
|
|
||||||
|
PMC->PMC_PCER0 |= 0x00200000L ; // Enable peripheral clock to SPI
|
||||||
|
/* Configure PIO */
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_ABCDSR[0] &= ~0x00007800 ; // Peripheral A bits 14,13,12,11
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~0x00007800 ; // Peripheral A
|
||||||
|
pioptr->PIO_PDR = 0x00007800 ; // Assign to peripheral
|
||||||
|
|
||||||
|
spiptr = SPI ;
|
||||||
|
timer = ( Master_frequency / 3000000 ) << 8 ; // Baud rate 3Mb/s
|
||||||
|
spiptr->SPI_MR = 0x14000011 ; // 0001 0100 0000 0000 0000 0000 0001 0001 Master
|
||||||
|
spiptr->SPI_CSR[0] = 0x01180009 | timer ; // 0000 0001 0001 1000 xxxx xxxx 0000 1001
|
||||||
|
NVIC_EnableIRQ(SPI_IRQn) ;
|
||||||
|
|
||||||
|
p = spi_buf ;
|
||||||
|
|
||||||
|
// *p = 0x39 ; // Unprotect sector command
|
||||||
|
// *(p+1) = 0 ;
|
||||||
|
// *(p+2) = 0 ;
|
||||||
|
// *(p+3) = 0 ; // 3 bytes address
|
||||||
|
|
||||||
|
// spi_operation( p, spi_buf, 4 ) ;
|
||||||
|
|
||||||
|
eeprom_write_enable() ;
|
||||||
|
|
||||||
|
*p = 1 ; // Write status register command
|
||||||
|
*(p+1) = 0 ;
|
||||||
|
spi_operation( p, spi_buf, 2 ) ;
|
||||||
|
}
|
||||||
|
|
||||||
|
// LCD i/o pins
|
||||||
|
// LCD_RES PC27
|
||||||
|
// LCD_CS1 PC26
|
||||||
|
// LCD_E PC12
|
||||||
|
// LCD_RnW PC13
|
||||||
|
// LCD_A0 PC15
|
||||||
|
// LCD_D0 PC0
|
||||||
|
// LCD_D1 PC7
|
||||||
|
// LCD_D2 PC6
|
||||||
|
// LCD_D3 PC5
|
||||||
|
// LCD_D4 PC4
|
||||||
|
// LCD_D5 PC3
|
||||||
|
// LCD_D6 PC2
|
||||||
|
// LCD_D7 PC1
|
||||||
|
|
||||||
|
#define LCD_DATA 0x000000FFL
|
||||||
|
#ifdef REVB
|
||||||
|
#define LCD_A0 0x00000080L
|
||||||
|
#else
|
||||||
|
#define LCD_A0 0x00008000L
|
||||||
|
#endif
|
||||||
|
#define LCD_RnW 0x00002000L
|
||||||
|
#define LCD_E 0x00001000L
|
||||||
|
#define LCD_CS1 0x04000000L
|
||||||
|
#define LCD_RES 0x08000000L
|
||||||
|
|
||||||
|
inline void lcd_init()
|
||||||
|
{
|
||||||
|
register Pio *pioptr ;
|
||||||
|
// /home/thus/txt/datasheets/lcd/KS0713.pdf
|
||||||
|
// ~/txt/flieger/ST7565RV17.pdf from http://www.glyn.de/content.asp?wdid=132&sid=
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr = PIOA ;
|
||||||
|
pioptr->PIO_PER = LCD_A0 ; // Enable bit 7 (LCD-A0)
|
||||||
|
pioptr->PIO_CODR = LCD_A0 ;
|
||||||
|
pioptr->PIO_OER = LCD_A0 ; // Set bit 7 output
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = 0x0C0030FFL ; // Enable bits 27,26,13,12,7-0
|
||||||
|
pioptr->PIO_CODR = LCD_E | LCD_RnW ;
|
||||||
|
pioptr->PIO_SODR = LCD_RES | LCD_CS1 ;
|
||||||
|
pioptr->PIO_OER = 0x0C0030FFL ; // Set bits 27,26,13,12,7-0 output
|
||||||
|
pioptr->PIO_OWER = 0x000000FFL ; // Allow write to ls 8 bits in ODSR
|
||||||
|
#else
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = 0x0C00B0FFL ; // Enable bits 27,26,15,13,12,7-0
|
||||||
|
pioptr->PIO_CODR = LCD_E | LCD_RnW | LCD_A0 ;
|
||||||
|
pioptr->PIO_SODR = LCD_RES | LCD_CS1 ;
|
||||||
|
pioptr->PIO_OER = 0x0C00B0FFL ; // Set bits 27,26,15,13,12,7-0 output
|
||||||
|
pioptr->PIO_OWER = 0x000000FFL ; // Allow write to ls 8 bits in ODSR
|
||||||
|
#endif
|
||||||
|
|
||||||
|
pioptr->PIO_CODR = LCD_RES ; // Reset LCD
|
||||||
|
TC0->TC_CHANNEL[0].TC_CCR = 5 ; // Enable clock and trigger it (may only need trigger)
|
||||||
|
while ( TC0->TC_CHANNEL[0].TC_CV < 12 ) // 2 uS, Value depends on MCK/2 (used 6MHz)
|
||||||
|
{
|
||||||
|
// Wait
|
||||||
|
}
|
||||||
|
pioptr->PIO_SODR = LCD_RES ; // Remove LCD reset
|
||||||
|
TC0->TC_CHANNEL[0].TC_CCR = 5 ; // Enable clock and trigger it (may only need trigger)
|
||||||
|
while ( TC0->TC_CHANNEL[0].TC_CV < 9000 ) // 1500 uS, Value depends on MCK/2 (used 6MHz)
|
||||||
|
{
|
||||||
|
// Wait
|
||||||
|
}
|
||||||
|
lcdSendCtl(0xe2); //Initialize the internal functions
|
||||||
|
lcdSendCtl(0xae); //DON = 0: display OFF
|
||||||
|
lcdSendCtl(0xa1); //ADC = 1: reverse direction(SEG132->SEG1)
|
||||||
|
lcdSendCtl(0xA6); //REV = 0: non-reverse display
|
||||||
|
lcdSendCtl(0xA4); //EON = 0: normal display. non-entire
|
||||||
|
lcdSendCtl(0xA2); // Select LCD bias=0
|
||||||
|
lcdSendCtl(0xC0); //SHL = 0: normal direction (COM1->COM64)
|
||||||
|
lcdSendCtl(0x2F); //Control power circuit operation VC=VR=VF=1
|
||||||
|
lcdSendCtl(0x25); //Select int resistance ratio R2 R1 R0 =5
|
||||||
|
lcdSendCtl(0x81); //Set reference voltage Mode
|
||||||
|
lcdSendCtl(0x22); // 24 SV5 SV4 SV3 SV2 SV1 SV0 = 0x18
|
||||||
|
lcdSendCtl(0xAF); //DON = 1: display ON
|
||||||
|
// g_eeGeneral.contrast = 0x22;
|
||||||
|
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_ODR = 0x0000003AL ; // Set bits 1, 3, 4, 5 input
|
||||||
|
pioptr->PIO_PUER = 0x0000003AL ; // Set bits 1, 3, 4, 5 with pullups
|
||||||
|
pioptr->PIO_ODSR = 0 ; // Drive D0 low
|
||||||
|
#else
|
||||||
|
pioptr->PIO_ODR = 0x0000003CL ; // Set bits 2, 3, 4, 5 input
|
||||||
|
pioptr->PIO_PUER = 0x0000003CL ; // Set bits 2, 3, 4, 5 with pullups
|
||||||
|
pioptr->PIO_ODSR = 0 ; // Drive D0 low
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void board_init()
|
||||||
|
{
|
||||||
|
register uint32_t goto_usb ;
|
||||||
|
register Pio *pioptr ;
|
||||||
|
// Debug variable
|
||||||
|
// uint32_t both_on ;
|
||||||
|
|
||||||
|
WDT->WDT_MR = 0x3FFFAFFF ; // Disable watchdog
|
||||||
|
|
||||||
|
MATRIX->CCFG_SYSIO |= 0x000000F0L ; // Disable syspins, enable B4,5,6,7
|
||||||
|
|
||||||
|
PMC->PMC_PCER0 = (1<<ID_PIOC)|(1<<ID_PIOB)|(1<<ID_PIOA)|(1<<ID_UART0) ; // Enable clocks to PIOB and PIOA and PIOC and UART0
|
||||||
|
pioptr = PIOA ;
|
||||||
|
#ifdef REVB
|
||||||
|
init_soft_power() ;
|
||||||
|
#else
|
||||||
|
// On REVB, PA21 is used as AD8, and measures current consumption.
|
||||||
|
pioptr->PIO_PER = PIO_PA21 ; // Enable bit A21 (EXT3)
|
||||||
|
pioptr->PIO_OER = PIO_PA21 ; // Set bit A21 as output
|
||||||
|
pioptr->PIO_SODR = PIO_PA21 ; // Set bit A21 ON
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// pioptr->PIO_PUER = 0x80000000 ; // Enable pullup on bit A31 (EXIT)
|
||||||
|
// pioptr->PIO_PER = 0x80000000 ; // Enable bit A31
|
||||||
|
|
||||||
|
pioptr = PIOC ;
|
||||||
|
pioptr->PIO_PER = PIO_PC25 ; // Enable bit C25 (USB-detect)
|
||||||
|
// pioptr->PIO_OER = 0x80000000L ; // Set bit C31 as output
|
||||||
|
// pioptr->PIO_SODR = 0x80000000L ; // Set bit C31
|
||||||
|
|
||||||
|
#ifndef REVB
|
||||||
|
// Configure RF_power (PC17) and PPM-jack-in (PC19), neither need pullups
|
||||||
|
pioptr->PIO_PER = 0x000A0000L ; // Enable bit C19, C17
|
||||||
|
pioptr->PIO_ODR = 0x000A0000L ; // Set bits C19 and C17 as input
|
||||||
|
#endif
|
||||||
|
|
||||||
|
config_free_pins() ;
|
||||||
|
|
||||||
|
// Next section configures the key inputs on the LCD data
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_PER = 0x0000003BL ; // Enable bits 1,3,4,5, 0
|
||||||
|
pioptr->PIO_OER = PIO_PC0 ; // Set bit 0 output
|
||||||
|
pioptr->PIO_ODR = 0x0000003AL ; // Set bits 1, 3, 4, 5 input
|
||||||
|
pioptr->PIO_PUER = 0x0000003AL ; // Set bits 1, 3, 4, 5 with pullups
|
||||||
|
#else
|
||||||
|
pioptr->PIO_PER = 0x0000003DL ; // Enable bits 2,3,4,5, 0
|
||||||
|
pioptr->PIO_OER = PIO_PC0 ; // Set bit 0 output
|
||||||
|
pioptr->PIO_ODR = 0x0000003CL ; // Set bits 2, 3, 4, 5 input
|
||||||
|
pioptr->PIO_PUER = 0x0000003CL ; // Set bits 2, 3, 4, 5 with pullups
|
||||||
|
#endif
|
||||||
|
|
||||||
|
pioptr = PIOB ;
|
||||||
|
#ifdef REVB
|
||||||
|
pioptr->PIO_PUER = PIO_PB5 ; // Enable pullup on bit B5 (MENU)
|
||||||
|
pioptr->PIO_PER = PIO_PB5 ; // Enable bit B5
|
||||||
|
#else
|
||||||
|
pioptr->PIO_PUER = PIO_PB6 ; // Enable pullup on bit B6 (MENU)
|
||||||
|
pioptr->PIO_PER = PIO_PB6 ; // Enable bit B6
|
||||||
|
#endif
|
||||||
|
|
||||||
|
setup_switches() ;
|
||||||
|
|
||||||
|
// Enable PCK2 on PB3, This is for testing of Timer 2 working
|
||||||
|
// It will be used as serial data to the Bluetooth module
|
||||||
|
pioptr->PIO_ABCDSR[0] |= PIO_PB3 ; // Peripheral B
|
||||||
|
pioptr->PIO_ABCDSR[1] &= ~PIO_PB3 ; // Peripheral B
|
||||||
|
pioptr->PIO_PDR = PIO_PB3 ; // Assign to peripheral
|
||||||
|
PMC->PMC_SCER |= 0x0400 ; // PCK2 enabled
|
||||||
|
PMC->PMC_PCK[2] = 2 ; // PCK2 is PLLA
|
||||||
|
|
||||||
|
UART_Configure( 9600, Master_frequency ) ;
|
||||||
|
UART2_Configure( 9600, Master_frequency ) ; // Testing
|
||||||
|
UART3_Configure( 9600, Master_frequency ) ; // Testing
|
||||||
|
|
||||||
|
start_timer2() ;
|
||||||
|
start_timer0() ;
|
||||||
|
init_adc() ;
|
||||||
|
init_pwm() ;
|
||||||
|
|
||||||
|
__enable_irq() ;
|
||||||
|
|
||||||
|
start_sound() ;
|
||||||
|
|
||||||
|
init_spi() ;
|
||||||
|
}
|
|
@ -47,4 +47,23 @@ inline void board_init()
|
||||||
DDRJ = 0x00; PORTJ = 0xff; // 7-0:Trim switch inputs
|
DDRJ = 0x00; PORTJ = 0xff; // 7-0:Trim switch inputs
|
||||||
DDRK = 0x00; PORTK = 0x00; // anain. No pull-ups!
|
DDRK = 0x00; PORTK = 0x00; // anain. No pull-ups!
|
||||||
DDRL = 0x80; PORTL = 0x7f; // 7: Hold_PWR_On (1=On, default Off), 6:Jack_Presence_TTL, 5-0: User Button inputs
|
DDRL = 0x80; PORTL = 0x7f; // 7: Hold_PWR_On (1=On, default Off), 6:Jack_Presence_TTL, 5-0: User Button inputs
|
||||||
|
|
||||||
|
ADMUX=ADC_VREF_TYPE;
|
||||||
|
ADCSRA=0x85; // ADC enabled, pre-scaler division=32 (no interrupt, no auto-triggering)
|
||||||
|
ADCSRB=(1<<MUX5);
|
||||||
|
|
||||||
|
/**** Set up timer/counter 0 ****/
|
||||||
|
/** Move old 64A Timer0 functions to Timer2 and use WGM on OC0(A) (PB7) for spkear tone output **/
|
||||||
|
|
||||||
|
// TCNT0 10ms = 16MHz/1024/156(.25) periodic timer (100ms interval)
|
||||||
|
// cycles at 9.984ms but includes 1:4 duty cycle correction to /157 to average at 10.0ms
|
||||||
|
TCCR2B = (0b111 << CS20); // Norm mode, clk/1024 (differs from ATmega64 chip)
|
||||||
|
OCR2A = 156;
|
||||||
|
TIMSK2 |= (1<<OCIE2A) | (1<<TOIE2); // Enable Output-Compare and Overflow interrrupts
|
||||||
|
|
||||||
|
// Set up Phase correct Waveform Gen. mode, at clk/64 = 250,000 counts/second
|
||||||
|
// (Higher speed allows for finer control of frquencies in the audio range.)
|
||||||
|
// Used for audio tone generation
|
||||||
|
TCCR0B = (1<<WGM02) | (0b011 << CS00);
|
||||||
|
TCCR0A = (0b01<<WGM00);
|
||||||
}
|
}
|
|
@ -43,4 +43,25 @@ inline void board_init()
|
||||||
DDRE = (1<<OUT_E_BUZZER); PORTE = 0xff-(1<<OUT_E_BUZZER); //pullups + buzzer 0
|
DDRE = (1<<OUT_E_BUZZER); PORTE = 0xff-(1<<OUT_E_BUZZER); //pullups + buzzer 0
|
||||||
DDRF = 0x00; PORTF = 0x00; //anain
|
DDRF = 0x00; PORTF = 0x00; //anain
|
||||||
DDRG = 0x14; PORTG = 0xfb; //pullups + SIM_CTL=1 = phonejack = ppm_in, Haptic output and off (0)
|
DDRG = 0x14; PORTG = 0xfb; //pullups + SIM_CTL=1 = phonejack = ppm_in, Haptic output and off (0)
|
||||||
|
|
||||||
|
ADMUX=ADC_VREF_TYPE;
|
||||||
|
ADCSRA=0x85; // ADC enabled, pre-scaler division=32 (no interrupt, no auto-triggering)
|
||||||
|
|
||||||
|
/**** Set up timer/counter 0 ****/
|
||||||
|
#if defined (AUDIO)
|
||||||
|
// TCNT0 10ms = 16MHz/1024/2(/78) periodic timer (for speaker tone generation)
|
||||||
|
// Capture ISR 7812.5/second -- runs per-10ms code segment once every 78
|
||||||
|
// cycles (9.984ms). Timer overflows at about 61Hz or once every 16ms.
|
||||||
|
TCCR0 = (0b111 << CS00);// Norm mode, clk/1024
|
||||||
|
OCR0 = 2;
|
||||||
|
#else
|
||||||
|
// TCNT0 10ms = 16MHz/1024/156 periodic timer (9.984ms)
|
||||||
|
// (with 1:4 duty at 157 to average 10.0ms)
|
||||||
|
// Timer overflows at about 61Hz or once every 16ms.
|
||||||
|
TCCR0 = (0b111 << CS00);// Norm mode, clk/1024
|
||||||
|
OCR0 = 156;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
TIMSK |= (1<<OCIE0) | (1<<TOIE0); // Enable Output-Compare and Overflow interrrupts
|
||||||
|
/********************************/
|
||||||
}
|
}
|
|
@ -37,7 +37,7 @@ uint16_t eeprom_pointer;
|
||||||
const char* eeprom_buffer_data;
|
const char* eeprom_buffer_data;
|
||||||
volatile int8_t eeprom_buffer_size = 0;
|
volatile int8_t eeprom_buffer_size = 0;
|
||||||
|
|
||||||
#ifndef SIMU
|
#if not defined(SIMU) and not defined(PCBARM)
|
||||||
|
|
||||||
inline void eeprom_write_byte()
|
inline void eeprom_write_byte()
|
||||||
{
|
{
|
||||||
|
@ -80,6 +80,8 @@ void eeWriteBlockCmp(const void *i_pointer_ram, uint16_t i_pointer_eeprom, size_
|
||||||
|
|
||||||
#ifdef SIMU
|
#ifdef SIMU
|
||||||
sem_post(&eeprom_write_sem);
|
sem_post(&eeprom_write_sem);
|
||||||
|
#elif defined (PCBARM)
|
||||||
|
|
||||||
#elif defined (PCBV4)
|
#elif defined (PCBV4)
|
||||||
EECR |= (1<<EERIE);
|
EECR |= (1<<EERIE);
|
||||||
#else
|
#else
|
||||||
|
|
|
@ -254,6 +254,8 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
frskyHubData.baroAltitude_bp += frskyHubData.baroAltitudeOffset;
|
frskyHubData.baroAltitude_bp += frskyHubData.baroAltitudeOffset;
|
||||||
if (frskyHubData.baroAltitude_bp > frskyHubData.maxAltitude)
|
if (frskyHubData.baroAltitude_bp > frskyHubData.maxAltitude)
|
||||||
frskyHubData.maxAltitude = frskyHubData.baroAltitude_bp;
|
frskyHubData.maxAltitude = frskyHubData.baroAltitude_bp;
|
||||||
|
if (frskyHubData.baroAltitude_bp < frskyHubData.minAltitude)
|
||||||
|
frskyHubData.minAltitude = frskyHubData.baroAltitude_bp;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case offsetof(FrskyHubData, gpsAltitude_ap):
|
case offsetof(FrskyHubData, gpsAltitude_ap):
|
||||||
|
@ -262,6 +264,8 @@ void parseTelemHubByte(uint8_t byte)
|
||||||
frskyHubData.gpsAltitude_bp += frskyHubData.gpsAltitudeOffset;
|
frskyHubData.gpsAltitude_bp += frskyHubData.gpsAltitudeOffset;
|
||||||
if (frskyHubData.gpsAltitude_bp > frskyHubData.maxAltitude)
|
if (frskyHubData.gpsAltitude_bp > frskyHubData.maxAltitude)
|
||||||
frskyHubData.maxAltitude = frskyHubData.gpsAltitude_bp;
|
frskyHubData.maxAltitude = frskyHubData.gpsAltitude_bp;
|
||||||
|
if (frskyHubData.gpsAltitude_bp < frskyHubData.minAltitude)
|
||||||
|
frskyHubData.minAltitude = frskyHubData.gpsAltitude_bp;
|
||||||
|
|
||||||
if (!frskyHubData.pilotLatitude && !frskyHubData.pilotLongitude) {
|
if (!frskyHubData.pilotLatitude && !frskyHubData.pilotLongitude) {
|
||||||
// First received GPS position => Pilot GPS position
|
// First received GPS position => Pilot GPS position
|
||||||
|
@ -692,9 +696,12 @@ void resetTelemetry()
|
||||||
|
|
||||||
frskyHubData.gpsAltitude_bp = 50;
|
frskyHubData.gpsAltitude_bp = 50;
|
||||||
frskyHubData.baroAltitude_bp = 50;
|
frskyHubData.baroAltitude_bp = 50;
|
||||||
|
frskyHubData.minAltitude = 10;
|
||||||
frskyHubData.maxAltitude = 500;
|
frskyHubData.maxAltitude = 500;
|
||||||
|
|
||||||
frskyHubData.accelY = 100;
|
frskyHubData.accelY = 100;
|
||||||
|
frskyHubData.temperature1 = -30;
|
||||||
|
frskyHubData.maxTemperature1 = 100;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -702,6 +709,8 @@ uint8_t maxTelemValue(uint8_t channel)
|
||||||
{
|
{
|
||||||
switch (channel) {
|
switch (channel) {
|
||||||
case TELEM_FUEL:
|
case TELEM_FUEL:
|
||||||
|
case TELEM_RSSI_TX:
|
||||||
|
case TELEM_RSSI_RX:
|
||||||
return 100;
|
return 100;
|
||||||
default:
|
default:
|
||||||
return 255;
|
return 255;
|
||||||
|
@ -756,12 +765,17 @@ static const pm_uint8_t bchunit_ar[] PROGMEM = {
|
||||||
void putsTelemetryChannel(uint8_t x, uint8_t y, uint8_t channel, int16_t val, uint8_t att)
|
void putsTelemetryChannel(uint8_t x, uint8_t y, uint8_t channel, int16_t val, uint8_t att)
|
||||||
{
|
{
|
||||||
switch (channel) {
|
switch (channel) {
|
||||||
|
case TELEM_TM1-1:
|
||||||
|
case TELEM_TM2-1:
|
||||||
|
putsTime(x-3*FW, y, val, att, att);
|
||||||
|
break;
|
||||||
case TELEM_MIN_A1-1:
|
case TELEM_MIN_A1-1:
|
||||||
case TELEM_MIN_A2-1:
|
case TELEM_MIN_A2-1:
|
||||||
channel -= TELEM_MIN_A1-1;
|
channel -= TELEM_MIN_A1-1-MAX_TIMERS;
|
||||||
// no break
|
// no break
|
||||||
case TELEM_A1-1:
|
case TELEM_A1-1:
|
||||||
case TELEM_A2-1:
|
case TELEM_A2-1:
|
||||||
|
channel -= MAX_TIMERS;
|
||||||
// A1 and A2
|
// A1 and A2
|
||||||
{
|
{
|
||||||
// TODO optimize this, avoid int32_t
|
// TODO optimize this, avoid int32_t
|
||||||
|
@ -792,8 +806,13 @@ void putsTelemetryChannel(uint8_t x, uint8_t y, uint8_t channel, int16_t val, ui
|
||||||
putsTelemetryValue(x, y, val, UNIT_RAW, att|PREC2);
|
putsTelemetryValue(x, y, val, UNIT_RAW, att|PREC2);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case TELEM_RSSI_TX-1:
|
||||||
|
case TELEM_RSSI_RX-1:
|
||||||
|
putsTelemetryValue(x, y, val, UNIT_RAW, att);
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
putsTelemetryValue(x, y, val, pgm_read_byte(bchunit_ar+channel-2), att);
|
putsTelemetryValue(x, y, val, pgm_read_byte(bchunit_ar+channel-6), att);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -906,15 +925,14 @@ void menuProcFrsky(uint8_t event)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (field) {
|
if (field) {
|
||||||
if (field >= TELEM_TM1) {
|
int16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+field-1);
|
||||||
uint8_t x, att;
|
uint8_t att = (i==3 ? NO_UNIT : DBLSIZE|NO_UNIT);
|
||||||
if (i==3) { x = j?80:20; att = 0; }
|
if (field <= TELEM_TM2) {
|
||||||
else { x = j?74:10; att = DBLSIZE; }
|
uint8_t x = (i==3 ? j?80:20 : j?74:10);
|
||||||
putsTime(x, 1+FH+2*FH*i, s_timerVal[field-TELEM_TM1], att, att);
|
putsTime(x, 1+FH+2*FH*i, value, att, att);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
int16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+field-1);
|
putsTelemetryChannel(j ? 128 : 63, i==3 ? 1+7*FH : 1+2*FH+2*FH*i, field-1, value, att);
|
||||||
putsTelemetryChannel(j ? 128 : 63, i==3 ? 1+7*FH : 1+2*FH+2*FH*i, field-1, value, i==3 ? NO_UNIT : DBLSIZE|NO_UNIT);
|
|
||||||
lcd_putsiAtt(j*65, 1+FH+2*FH*i, STR_VTELEMCHNS, field, 0);
|
lcd_putsiAtt(j*65, 1+FH+2*FH*i, STR_VTELEMCHNS, field, 0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -932,13 +950,17 @@ void menuProcFrsky(uint8_t event)
|
||||||
if (source && bmax > bmin) {
|
if (source && bmax > bmin) {
|
||||||
lcd_putsiAtt(0, bars_height+bars_height+1+i*(bars_height+6), STR_VTELEMCHNS, source, 0);
|
lcd_putsiAtt(0, bars_height+bars_height+1+i*(bars_height+6), STR_VTELEMCHNS, source, 0);
|
||||||
lcd_rect(25, bars_height+6+i*(bars_height+6), 101, bars_height+2);
|
lcd_rect(25, bars_height+6+i*(bars_height+6), 101, bars_height+2);
|
||||||
uint16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+source-1);
|
uint16_t value = getValue(CSW_CHOUT_BASE+NUM_CHNOUT+source-1);
|
||||||
uint16_t threshold = 0;
|
uint16_t threshold = 0;
|
||||||
uint8_t thresholdX = 0;
|
uint8_t thresholdX = 0;
|
||||||
if (source <= 2)
|
if (source <= TELEM_TM1)
|
||||||
threshold = g_model.frsky.channels[source-1].alarms_value[0];
|
threshold = 0;
|
||||||
|
else if (source <= TELEM_A2)
|
||||||
|
threshold = g_model.frsky.channels[source-TELEM_A1].alarms_value[0];
|
||||||
|
else if (source <= TELEM_RSSI_RX)
|
||||||
|
threshold = 0;
|
||||||
else
|
else
|
||||||
threshold = convertTelemValue(source, barsThresholds[source-3]);
|
threshold = convertTelemValue(source, barsThresholds[source-TELEM_ALT]);
|
||||||
int16_t barMin = convertTelemValue(source, bmin);
|
int16_t barMin = convertTelemValue(source, bmin);
|
||||||
int16_t barMax = convertTelemValue(source, bmax);
|
int16_t barMax = convertTelemValue(source, bmax);
|
||||||
if (threshold) {
|
if (threshold) {
|
||||||
|
@ -973,9 +995,9 @@ void menuProcFrsky(uint8_t event)
|
||||||
if (g_model.frsky.channels[i].ratio) {
|
if (g_model.frsky.channels[i].ratio) {
|
||||||
blink = (FRSKY_alarmRaised(i) ? INVERS : 0);
|
blink = (FRSKY_alarmRaised(i) ? INVERS : 0);
|
||||||
putsStrIdx(0, y, STR_A, i+1, TWO_DOTS);
|
putsStrIdx(0, y, STR_A, i+1, TWO_DOTS);
|
||||||
putsTelemetryChannel(3*FW, y, i, frskyTelemetry[i].value, blink|DBLSIZE|LEFT);
|
putsTelemetryChannel(3*FW, y, i+MAX_TIMERS, frskyTelemetry[i].value, blink|DBLSIZE|LEFT);
|
||||||
lcd_putc(12*FW-1, y-FH, '<'); putsTelemetryChannel(17*FW, y-FH, i, frskyTelemetry[i].min, NO_UNIT);
|
lcd_putc(12*FW-1, y-FH, '<'); putsTelemetryChannel(17*FW, y-FH, i+MAX_TIMERS, frskyTelemetry[i].min, NO_UNIT);
|
||||||
lcd_putc(12*FW, y, '>'); putsTelemetryChannel(17*FW, y, i, frskyTelemetry[i].max, NO_UNIT);
|
lcd_putc(12*FW, y, '>'); putsTelemetryChannel(17*FW, y, i+MAX_TIMERS, frskyTelemetry[i].max, NO_UNIT);
|
||||||
y += 3*FH;
|
y += 3*FH;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -117,6 +117,7 @@ PACK(struct FrskyHubData {
|
||||||
int16_t gpsAltitudeOffset;
|
int16_t gpsAltitudeOffset;
|
||||||
uint8_t minCellMinVolts;
|
uint8_t minCellMinVolts;
|
||||||
/* order is important starting from here */
|
/* order is important starting from here */
|
||||||
|
int16_t minAltitude;
|
||||||
int16_t maxAltitude;
|
int16_t maxAltitude;
|
||||||
uint16_t maxRpm;
|
uint16_t maxRpm;
|
||||||
int16_t maxTemperature1;
|
int16_t maxTemperature1;
|
||||||
|
|
|
@ -432,6 +432,8 @@ void lcd_filled_rect(uint8_t x, int8_t y, uint8_t w, uint8_t h, uint8_t pat, uin
|
||||||
|
|
||||||
void putsTime(uint8_t x,uint8_t y,int16_t tme,uint8_t att,uint8_t att2)
|
void putsTime(uint8_t x,uint8_t y,int16_t tme,uint8_t att,uint8_t att2)
|
||||||
{
|
{
|
||||||
|
if (att & LEFT) x+=2*FW;
|
||||||
|
|
||||||
if (tme<0) {
|
if (tme<0) {
|
||||||
lcd_putcAtt(x - ((att & DBLSIZE) ? FW+1 : FWNUM), y, '-', att);
|
lcd_putcAtt(x - ((att & DBLSIZE) ? FW+1 : FWNUM), y, '-', att);
|
||||||
tme = -tme;
|
tme = -tme;
|
||||||
|
@ -471,11 +473,9 @@ void putsChnRaw(uint8_t x, uint8_t y, uint8_t idx, uint8_t att)
|
||||||
putsStrIdx(x, y, STR_PPM, idx - (NUM_STICKS+NUM_POTS+2+3), att);
|
putsStrIdx(x, y, STR_PPM, idx - (NUM_STICKS+NUM_POTS+2+3), att);
|
||||||
else if (idx<=NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT)
|
else if (idx<=NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT)
|
||||||
putsStrIdx(x, y, STR_CH, idx - (NUM_STICKS+NUM_POTS+2+3+NUM_PPM), att);
|
putsStrIdx(x, y, STR_CH, idx - (NUM_STICKS+NUM_POTS+2+3+NUM_PPM), att);
|
||||||
else if (idx<=NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT+MAX_TIMERS)
|
|
||||||
putsStrIdx(x, y, STR_TMR, idx - (NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT), att);
|
|
||||||
#ifdef FRSKY
|
#ifdef FRSKY
|
||||||
else
|
else
|
||||||
lcd_putsiAtt(x, y, STR_VTELEMCHNS, idx-(NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT+MAX_TIMERS), att);
|
lcd_putsiAtt(x, y, STR_VTELEMCHNS, idx-(NUM_STICKS+NUM_POTS+2+3+NUM_PPM+NUM_CHNOUT), att);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1678,16 +1678,16 @@ void menuProcCustomSwitches(uint8_t event)
|
||||||
lcd_putsiAtt(4*FW - 1, y, STR_VCSWFUNC, cs.func, m_posHorz==0 ? attr : 0);
|
lcd_putsiAtt(4*FW - 1, y, STR_VCSWFUNC, cs.func, m_posHorz==0 ? attr : 0);
|
||||||
|
|
||||||
uint8_t cstate = CS_STATE(cs.func);
|
uint8_t cstate = CS_STATE(cs.func);
|
||||||
int8_t v1_min=0, v1_max=NUM_XCHNCSW, v2_min=0, v2_max=NUM_XCHNCSW;
|
int8_t v1_min=0, v1_max=NUM_XCHNCSW-MAX_TIMERS/*TODO why*/, v2_min=0, v2_max=NUM_XCHNCSW;
|
||||||
|
|
||||||
if (cstate == CS_VOFS)
|
if (cstate == CS_VOFS)
|
||||||
{
|
{
|
||||||
putsChnRaw(12*FW-2, y, cs.v1, (m_posHorz==1 ? attr : 0));
|
putsChnRaw(12*FW-2, y, cs.v1, (m_posHorz==1 ? attr : 0));
|
||||||
|
|
||||||
#if defined(FRSKY)
|
#if defined(FRSKY)
|
||||||
if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY) {
|
if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS) {
|
||||||
putsTelemetryChannel(20*FW, y, cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY+1), convertTelemValue(cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY), 128+cs.v2), m_posHorz==2 ? attr : 0);
|
putsTelemetryChannel(20*FW, y, cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS+1), convertTelemValue(cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS), 128+cs.v2), m_posHorz==2 ? attr : 0);
|
||||||
v2_min = -128; v2_max = maxTelemValue(cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY)) - 128;
|
v2_min = -128; v2_max = maxTelemValue(cs.v1 - (NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS)) - 128;
|
||||||
if (cs.v2 > v2_max) {
|
if (cs.v2 > v2_max) {
|
||||||
cs.v2 = v2_max;
|
cs.v2 = v2_max;
|
||||||
eeDirty(EE_MODEL);
|
eeDirty(EE_MODEL);
|
||||||
|
@ -1695,11 +1695,7 @@ void menuProcCustomSwitches(uint8_t event)
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
#endif
|
#endif
|
||||||
if (cs.v1 > NUM_XCHNCSW-NUM_TELEMETRY-MAX_TIMERS) {
|
{
|
||||||
putsTime(17*FW, y, 98+cs.v2, m_posHorz==2 ? attr : 0, m_posHorz==2 ? attr : 0); // TODO optim
|
|
||||||
v2_min = -128; v2_max = 127;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
lcd_outdezAtt(20*FW, y, cs.v2, m_posHorz==2 ? attr : 0);
|
lcd_outdezAtt(20*FW, y, cs.v2, m_posHorz==2 ? attr : 0);
|
||||||
v2_min = -125; v2_max = 125;
|
v2_min = -125; v2_max = 125;
|
||||||
}
|
}
|
||||||
|
@ -1728,18 +1724,7 @@ void menuProcCustomSwitches(uint8_t event)
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
{
|
{
|
||||||
int8_t v1 = cs.v1;
|
|
||||||
CHECK_INCDEC_MODELVAR(event, cs.v1, v1_min, v1_max);
|
CHECK_INCDEC_MODELVAR(event, cs.v1, v1_min, v1_max);
|
||||||
if (cstate == CS_VOFS) {
|
|
||||||
if (cs.v1 == CSW_CHOUT_BASE+NUM_CHNOUT+1 && v1 < cs.v1) cs.v2 = -98;
|
|
||||||
#ifdef FRSKY
|
|
||||||
if (cs.v1 == CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+1 && v1 < cs.v1) cs.v2 = -128;
|
|
||||||
#endif
|
|
||||||
if (cs.v1 == CSW_CHOUT_BASE+NUM_CHNOUT && v1 > cs.v1) cs.v2 = 0;
|
|
||||||
#ifdef FRSKY
|
|
||||||
if (cs.v1 == CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS && v1 > cs.v1) cs.v2 = -98;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case 2:
|
case 2:
|
||||||
|
@ -1879,7 +1864,7 @@ void menuProcTelemetry(uint8_t event)
|
||||||
if(s_pgOfs<subN) {
|
if(s_pgOfs<subN) {
|
||||||
y=(subN-s_pgOfs)*FH;
|
y=(subN-s_pgOfs)*FH;
|
||||||
lcd_puts(4, y, STR_RANGE);
|
lcd_puts(4, y, STR_RANGE);
|
||||||
putsTelemetryChannel(TELEM_COL2, y, i, 255-g_model.frsky.channels[i].offset, (sub==subN && m_posHorz==0 ? blink:0)|NO_UNIT|LEFT);
|
putsTelemetryChannel(TELEM_COL2, y, i+MAX_TIMERS, 255-g_model.frsky.channels[i].offset, (sub==subN && m_posHorz==0 ? blink:0)|NO_UNIT|LEFT);
|
||||||
lcd_putsiAtt(lcd_lastPos+1, y, STR_VTELEMUNIT, g_model.frsky.channels[i].type, (sub==subN && m_posHorz==1 ? blink:0));
|
lcd_putsiAtt(lcd_lastPos+1, y, STR_VTELEMUNIT, g_model.frsky.channels[i].type, (sub==subN && m_posHorz==1 ? blink:0));
|
||||||
if (sub==subN && (s_editMode>0 || p1valdiff)) {
|
if (sub==subN && (s_editMode>0 || p1valdiff)) {
|
||||||
if (m_posHorz == 0) {
|
if (m_posHorz == 0) {
|
||||||
|
@ -1906,7 +1891,7 @@ void menuProcTelemetry(uint8_t event)
|
||||||
if(s_pgOfs<subN) {
|
if(s_pgOfs<subN) {
|
||||||
y=(subN-s_pgOfs)*FH;
|
y=(subN-s_pgOfs)*FH;
|
||||||
lcd_puts(4, y, STR_OFFSET);
|
lcd_puts(4, y, STR_OFFSET);
|
||||||
putsTelemetryChannel(TELEM_COL2, y, i, 0, (sub==subN ? blink:0)|LEFT);
|
putsTelemetryChannel(TELEM_COL2, y, i+MAX_TIMERS, 0, (sub==subN ? blink:0)|LEFT);
|
||||||
if(sub==subN) CHECK_INCDEC_MODELVAR(event, g_model.frsky.channels[i].offset, -128, 127);
|
if(sub==subN) CHECK_INCDEC_MODELVAR(event, g_model.frsky.channels[i].offset, -128, 127);
|
||||||
}
|
}
|
||||||
subN++;
|
subN++;
|
||||||
|
@ -1917,7 +1902,7 @@ void menuProcTelemetry(uint8_t event)
|
||||||
lcd_puts(4, y, STR_ALARM);
|
lcd_puts(4, y, STR_ALARM);
|
||||||
lcd_putsiAtt(TELEM_COL2, y, STR_VALARM, ALARM_LEVEL(i, j), (sub==subN && m_posHorz==0) ? blink : 0);
|
lcd_putsiAtt(TELEM_COL2, y, STR_VALARM, ALARM_LEVEL(i, j), (sub==subN && m_posHorz==0) ? blink : 0);
|
||||||
lcd_putsiAtt(TELEM_COL2+4*FW, y, STR_VALARMFN, ALARM_GREATER(i, j), (sub==subN && m_posHorz==1) ? blink : 0);
|
lcd_putsiAtt(TELEM_COL2+4*FW, y, STR_VALARMFN, ALARM_GREATER(i, j), (sub==subN && m_posHorz==1) ? blink : 0);
|
||||||
putsTelemetryChannel(TELEM_COL2+6*FW, y, i, g_model.frsky.channels[i].alarms_value[j], (sub==subN && m_posHorz==2 ? blink:0) | LEFT);
|
putsTelemetryChannel(TELEM_COL2+6*FW, y, i+MAX_TIMERS, g_model.frsky.channels[i].alarms_value[j], (sub==subN && m_posHorz==2 ? blink:0) | LEFT);
|
||||||
|
|
||||||
if(sub==subN && (s_editMode>0 || p1valdiff)) {
|
if(sub==subN && (s_editMode>0 || p1valdiff)) {
|
||||||
switch (m_posHorz) {
|
switch (m_posHorz) {
|
||||||
|
|
|
@ -272,8 +272,12 @@ PACK(typedef struct t_FrSkyChannelData {
|
||||||
|
|
||||||
enum TelemetrySource {
|
enum TelemetrySource {
|
||||||
TELEM_NONE,
|
TELEM_NONE,
|
||||||
|
TELEM_TM1,
|
||||||
|
TELEM_TM2,
|
||||||
TELEM_A1,
|
TELEM_A1,
|
||||||
TELEM_A2,
|
TELEM_A2,
|
||||||
|
TELEM_RSSI_TX,
|
||||||
|
TELEM_RSSI_RX,
|
||||||
TELEM_ALT,
|
TELEM_ALT,
|
||||||
TELEM_RPM,
|
TELEM_RPM,
|
||||||
TELEM_FUEL,
|
TELEM_FUEL,
|
||||||
|
@ -282,27 +286,24 @@ enum TelemetrySource {
|
||||||
TELEM_SPEED,
|
TELEM_SPEED,
|
||||||
TELEM_DIST,
|
TELEM_DIST,
|
||||||
TELEM_CELL,
|
TELEM_CELL,
|
||||||
TELEM_RSSI_TX,
|
|
||||||
TELEM_RSSI_RX,
|
|
||||||
TELEM_ACCx,
|
TELEM_ACCx,
|
||||||
TELEM_ACCy,
|
TELEM_ACCy,
|
||||||
TELEM_ACCz,
|
TELEM_ACCz,
|
||||||
TELEM_HDG,
|
TELEM_HDG,
|
||||||
TELEM_MIN_A1,
|
TELEM_MIN_A1,
|
||||||
TELEM_MIN_A2,
|
TELEM_MIN_A2,
|
||||||
|
TELEM_MIN_ALT,
|
||||||
TELEM_MAX_ALT,
|
TELEM_MAX_ALT,
|
||||||
TELEM_MAX_RPM,
|
TELEM_MAX_RPM,
|
||||||
TELEM_MAX_T1,
|
TELEM_MAX_T1,
|
||||||
TELEM_MAX_T2,
|
TELEM_MAX_T2,
|
||||||
TELEM_MAX_SPEED,
|
TELEM_MAX_SPEED,
|
||||||
TELEM_MAX_DIST,
|
TELEM_MAX_DIST,
|
||||||
TELEM_TM1,
|
|
||||||
TELEM_TM2,
|
|
||||||
TELEM_ACC,
|
TELEM_ACC,
|
||||||
TELEM_GPS_TIME,
|
TELEM_GPS_TIME,
|
||||||
TELEM_BAR_MAX = TELEM_CELL,
|
TELEM_BAR_MAX = TELEM_CELL,
|
||||||
TELEM_CSW_MAX = TELEM_CELL,
|
TELEM_CSW_MAX = TELEM_CELL,
|
||||||
TELEM_DISPLAY_MAX = TELEM_TM2,
|
TELEM_DISPLAY_MAX = TELEM_MAX_DIST,
|
||||||
TELEM_STATUS_MAX = TELEM_GPS_TIME
|
TELEM_STATUS_MAX = TELEM_GPS_TIME
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -206,7 +206,6 @@ const pm_char STR_FREESTACKMINB[] PROGMEM = TR_FREESTACKMINB;
|
||||||
const pm_char STR_MENUTORESET[] PROGMEM = TR_MENUTORESET;
|
const pm_char STR_MENUTORESET[] PROGMEM = TR_MENUTORESET;
|
||||||
const pm_char STR_PPM[] PROGMEM = TR_PPM;
|
const pm_char STR_PPM[] PROGMEM = TR_PPM;
|
||||||
const pm_char STR_CH[] PROGMEM = TR_CH;
|
const pm_char STR_CH[] PROGMEM = TR_CH;
|
||||||
const pm_char STR_TMR[] PROGMEM = TR_TMR;
|
|
||||||
const pm_char STR_MODEL[] PROGMEM = TR_MODEL;
|
const pm_char STR_MODEL[] PROGMEM = TR_MODEL;
|
||||||
const pm_char STR_FP[] PROGMEM = TR_FP;
|
const pm_char STR_FP[] PROGMEM = TR_FP;
|
||||||
const pm_char STR_EEPROMLOWMEM[] PROGMEM = TR_EEPROMLOWMEM;
|
const pm_char STR_EEPROMLOWMEM[] PROGMEM = TR_EEPROMLOWMEM;
|
||||||
|
|
|
@ -285,7 +285,6 @@ extern const pm_char STR_FREESTACKMINB[];
|
||||||
extern const pm_char STR_MENUTORESET[];
|
extern const pm_char STR_MENUTORESET[];
|
||||||
extern const pm_char STR_PPM[];
|
extern const pm_char STR_PPM[];
|
||||||
extern const pm_char STR_CH[];
|
extern const pm_char STR_CH[];
|
||||||
extern const pm_char STR_TMR[];
|
|
||||||
extern const pm_char STR_MODEL[];
|
extern const pm_char STR_MODEL[];
|
||||||
extern const pm_char STR_FP[];
|
extern const pm_char STR_FP[];
|
||||||
extern const pm_char STR_EEPROMLOWMEM[];
|
extern const pm_char STR_EEPROMLOWMEM[];
|
||||||
|
|
|
@ -307,29 +307,29 @@ int16_t getValue(uint8_t i)
|
||||||
else if(i<CSW_PPM_BASE+NUM_CAL_PPM) return (g_ppmIns[i-CSW_PPM_BASE] - g_eeGeneral.trainer.calib[i-CSW_PPM_BASE])*2;
|
else if(i<CSW_PPM_BASE+NUM_CAL_PPM) return (g_ppmIns[i-CSW_PPM_BASE] - g_eeGeneral.trainer.calib[i-CSW_PPM_BASE])*2;
|
||||||
else if(i<CSW_PPM_BASE+NUM_PPM) return g_ppmIns[i-CSW_PPM_BASE]*2;
|
else if(i<CSW_PPM_BASE+NUM_PPM) return g_ppmIns[i-CSW_PPM_BASE]*2;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT) return ex_chans[i-CSW_CHOUT_BASE];
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT) return ex_chans[i-CSW_CHOUT_BASE];
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS) return s_timerVal[i-CSW_CHOUT_BASE-NUM_CHNOUT-1];
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_TM2) return s_timerVal[i-CSW_CHOUT_BASE-NUM_CHNOUT];
|
||||||
#if defined(FRSKY)
|
#if defined(FRSKY)
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+2) return frskyTelemetry[i-CSW_CHOUT_BASE-NUM_CHNOUT-MAX_TIMERS].value;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_A2) return frskyTelemetry[i-CSW_CHOUT_BASE-NUM_CHNOUT-2].value;
|
||||||
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_RSSI_TX) return frskyRSSI[1].value;
|
||||||
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_RSSI_RX) return frskyRSSI[0].value;
|
||||||
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
|
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+3) return frskyHubData.baroAltitude_bp;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ALT) return frskyHubData.baroAltitude_bp;
|
||||||
#endif
|
#endif
|
||||||
#if defined(FRSKY_HUB)
|
#if defined(FRSKY_HUB)
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_RPM) return frskyHubData.rpm;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_RPM) return frskyHubData.rpm;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_FUEL) return frskyHubData.fuelLevel;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_FUEL) return frskyHubData.fuelLevel;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_T1) return frskyHubData.temperature1;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_T1) return frskyHubData.temperature1;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_T2) return frskyHubData.temperature2;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_T2) return frskyHubData.temperature2;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_SPEED) return frskyHubData.gpsSpeed_ap;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_SPEED) return frskyHubData.gpsSpeed_ap;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_DIST) return frskyHubData.gpsDistance;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_DIST) return frskyHubData.gpsDistance;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_CELL) return (int16_t)frskyHubData.minCellVolts * 2;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_CELL) return (int16_t)frskyHubData.minCellVolts * 2;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_RSSI_TX) return frskyRSSI[1].value;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ACCx) return frskyHubData.accelX;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_RSSI_RX) return frskyRSSI[0].value;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ACCy) return frskyHubData.accelY;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_ACCx) return frskyHubData.accelX;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ACCz) return frskyHubData.accelZ;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_ACCy) return frskyHubData.accelY;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_HDG) return frskyHubData.gpsCourse_bp;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_ACCz) return frskyHubData.accelZ;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_MIN_A1) return frskyTelemetry[0].min;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_HDG) return frskyHubData.gpsCourse_bp;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_MIN_A2) return frskyTelemetry[1].min;
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_MIN_A1) return frskyTelemetry[0].min;
|
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_MAX_DIST) return *(((int16_t*)(&frskyHubData.minAltitude))+i-(CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_MIN_ALT-1));
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_MIN_A2) return frskyTelemetry[1].min;
|
|
||||||
else if(i<CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_MAX_DIST) return *(((int16_t*)(&frskyHubData.maxGpsDistance))-i-(CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+TELEM_MAX_DIST-1));
|
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
else return 0;
|
else return 0;
|
||||||
|
@ -392,20 +392,18 @@ bool __getSwitch(int8_t swtch)
|
||||||
if (s == CS_VOFS) {
|
if (s == CS_VOFS) {
|
||||||
#if defined(FRSKY)
|
#if defined(FRSKY)
|
||||||
// Telemetry
|
// Telemetry
|
||||||
if (cs.v1 > CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS) {
|
if (cs.v1 > CSW_CHOUT_BASE+NUM_CHNOUT) {
|
||||||
y = convertTelemValue(cs.v1-(CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS), 128+cs.v2);
|
y = convertTelemValue(cs.v1-(CSW_CHOUT_BASE+NUM_CHNOUT), 128+cs.v2);
|
||||||
if (cs.v1 > CSW_CHOUT_BASE+NUM_CHNOUT+MAX_TIMERS+2/*A1 and A2*/) {
|
if (cs.v1 >= CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ALT) {
|
||||||
// Fill the threshold array
|
// Fill the threshold array
|
||||||
barsThresholds[cs.v1-CSW_CHOUT_BASE-NUM_CHNOUT-MAX_TIMERS-3] = 128 + cs.v2;
|
barsThresholds[cs.v1-CSW_CHOUT_BASE-NUM_CHNOUT-TELEM_ALT] = 128 + cs.v2;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
#endif
|
#endif
|
||||||
// Timers
|
{
|
||||||
if (cs.v1 > CSW_CHOUT_BASE+NUM_CHNOUT)
|
|
||||||
y = 98+cs.v2;
|
|
||||||
else
|
|
||||||
y = calc100toRESX(cs.v2);
|
y = calc100toRESX(cs.v2);
|
||||||
|
}
|
||||||
|
|
||||||
switch (cs.func) {
|
switch (cs.func) {
|
||||||
case CS_VPOS:
|
case CS_VPOS:
|
||||||
|
@ -809,7 +807,6 @@ uint16_t anaIn(uint8_t chan)
|
||||||
return *p;
|
return *p;
|
||||||
}
|
}
|
||||||
|
|
||||||
#define ADC_VREF_TYPE 0x40 // AVCC with external capacitor at AREF pin
|
|
||||||
void getADC_filt()
|
void getADC_filt()
|
||||||
{
|
{
|
||||||
static uint16_t t_ana[2][8];
|
static uint16_t t_ana[2][8];
|
||||||
|
@ -2105,49 +2102,6 @@ int main(void)
|
||||||
|
|
||||||
lcd_init();
|
lcd_init();
|
||||||
|
|
||||||
ADMUX=ADC_VREF_TYPE;
|
|
||||||
ADCSRA=0x85; // ADC enabled, pre-scaler division=32 (no interrupt, no auto-triggering)
|
|
||||||
#if defined (PCBV4)
|
|
||||||
ADCSRB=(1<<MUX5);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/**** Set up timer/counter 0 ****/
|
|
||||||
#if defined (PCBV4)
|
|
||||||
/** Move old 64A Timer0 functions to Timer2 and use WGM on OC0(A) (PB7) for spkear tone output **/
|
|
||||||
|
|
||||||
// TCNT0 10ms = 16MHz/1024/156(.25) periodic timer (100ms interval)
|
|
||||||
// cycles at 9.984ms but includes 1:4 duty cycle correction to /157 to average at 10.0ms
|
|
||||||
TCCR2B = (0b111 << CS20); // Norm mode, clk/1024 (differs from ATmega64 chip)
|
|
||||||
OCR2A = 156;
|
|
||||||
TIMSK2 |= (1<<OCIE2A) | (1<<TOIE2); // Enable Output-Compare and Overflow interrrupts
|
|
||||||
|
|
||||||
// Set up Phase correct Waveform Gen. mode, at clk/64 = 250,000 counts/second
|
|
||||||
// (Higher speed allows for finer control of frquencies in the audio range.)
|
|
||||||
// Used for audio tone generation
|
|
||||||
TCCR0B = (1<<WGM02) | (0b011 << CS00);
|
|
||||||
TCCR0A = (0b01<<WGM00);
|
|
||||||
|
|
||||||
#else
|
|
||||||
|
|
||||||
# if defined (AUDIO)
|
|
||||||
// TCNT0 10ms = 16MHz/1024/2(/78) periodic timer (for speaker tone generation)
|
|
||||||
// Capture ISR 7812.5/second -- runs per-10ms code segment once every 78
|
|
||||||
// cycles (9.984ms). Timer overflows at about 61Hz or once every 16ms.
|
|
||||||
TCCR0 = (0b111 << CS00);// Norm mode, clk/1024
|
|
||||||
OCR0 = 2;
|
|
||||||
# else
|
|
||||||
// TCNT0 10ms = 16MHz/1024/156 periodic timer (9.984ms)
|
|
||||||
// (with 1:4 duty at 157 to average 10.0ms)
|
|
||||||
// Timer overflows at about 61Hz or once every 16ms.
|
|
||||||
TCCR0 = (0b111 << CS00);// Norm mode, clk/1024
|
|
||||||
OCR0 = 156;
|
|
||||||
# endif
|
|
||||||
|
|
||||||
TIMSK |= (1<<OCIE0) | (1<<TOIE0); // Enable Output-Compare and Overflow interrrupts
|
|
||||||
/********************************/
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Init Stack while interrupts are disabled
|
// Init Stack while interrupts are disabled
|
||||||
#define STACKPTR _SFR_IO16(0x3D)
|
#define STACKPTR _SFR_IO16(0x3D)
|
||||||
{
|
{
|
||||||
|
|
36
src/open9x.h
36
src/open9x.h
|
@ -39,20 +39,25 @@
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
#if defined(PCBV4)
|
#if defined(PCBV4)
|
||||||
#include "ff.h"
|
#include "ff.h"
|
||||||
#include "gtime.h"
|
#include "gtime.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef SIMU
|
#if defined(SIMU)
|
||||||
#include "simpgmspace.h"
|
#include "simpgmspace.h"
|
||||||
|
#elif defined(PCBARM)
|
||||||
|
typedef const unsigned char pm_uchar;
|
||||||
|
typedef const char pm_char;
|
||||||
|
typedef const uint16_t pm_uint16_t;
|
||||||
|
typedef const uint8_t pm_uint8_t;
|
||||||
|
typedef const int16_t pm_int16_t;
|
||||||
|
typedef const int8_t pm_int8_t;
|
||||||
|
#define wdt_reset()
|
||||||
#else
|
#else
|
||||||
///opt/cross/avr/include/avr/pgmspace.h
|
|
||||||
#include <stddef.h>
|
|
||||||
#include <avr/io.h>
|
#include <avr/io.h>
|
||||||
#define assert(x)
|
|
||||||
|
|
||||||
#include <avr/pgmspace.h>
|
#include <avr/pgmspace.h>
|
||||||
#include "pgmtypes.h"
|
#include "pgmtypes.h"
|
||||||
|
|
||||||
|
@ -63,13 +68,17 @@
|
||||||
#include <util/delay.h>
|
#include <util/delay.h>
|
||||||
#define pgm_read_adr(address_short) pgm_read_word(address_short)
|
#define pgm_read_adr(address_short) pgm_read_word(address_short)
|
||||||
#include <avr/wdt.h>
|
#include <avr/wdt.h>
|
||||||
#define printf printf_not_allowed
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include "file.h"
|
#include "file.h"
|
||||||
#include "lcd.h"
|
#include "lcd.h"
|
||||||
#include "myeeprom.h"
|
#include "myeeprom.h"
|
||||||
|
|
||||||
|
#if not defined(SIMU)
|
||||||
|
#define assert(x)
|
||||||
|
#define printf printf_not_allowed
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef JETI
|
#ifdef JETI
|
||||||
// Jeti-DUPLEX Telemetry
|
// Jeti-DUPLEX Telemetry
|
||||||
extern uint16_t jeti_keys;
|
extern uint16_t jeti_keys;
|
||||||
|
@ -407,8 +416,6 @@ extern uint8_t pxxFlag;
|
||||||
#define PXX_SEND_RXNUM 0x01
|
#define PXX_SEND_RXNUM 0x01
|
||||||
#define PXX_SEND_FAILSAFE 0x02 // TODO where is it used?
|
#define PXX_SEND_FAILSAFE 0x02 // TODO where is it used?
|
||||||
|
|
||||||
typedef void (*getADCp)();
|
|
||||||
|
|
||||||
#define ZCHAR_MAX (40 + LEN_SPECIAL_CHARS)
|
#define ZCHAR_MAX (40 + LEN_SPECIAL_CHARS)
|
||||||
|
|
||||||
extern char idx2char(int8_t idx);
|
extern char idx2char(int8_t idx);
|
||||||
|
@ -489,6 +496,8 @@ void checkTHR();
|
||||||
void checkSwitches();
|
void checkSwitches();
|
||||||
void checkAlarm();
|
void checkAlarm();
|
||||||
|
|
||||||
|
#define ADC_VREF_TYPE 0x40 // AVCC with external capacitor at AREF pin
|
||||||
|
|
||||||
#define GETADC_SING = 0
|
#define GETADC_SING = 0
|
||||||
#define GETADC_OSMP = 1
|
#define GETADC_OSMP = 1
|
||||||
#define GETADC_FILT = 2
|
#define GETADC_FILT = 2
|
||||||
|
@ -497,6 +506,8 @@ void getADC_single();
|
||||||
void getADC_osmp();
|
void getADC_osmp();
|
||||||
void getADC_filt();
|
void getADC_filt();
|
||||||
|
|
||||||
|
typedef void (*getADCp)();
|
||||||
|
|
||||||
// checkIncDec flags
|
// checkIncDec flags
|
||||||
#define EE_GENERAL 0x01
|
#define EE_GENERAL 0x01
|
||||||
#define EE_MODEL 0x02
|
#define EE_MODEL 0x02
|
||||||
|
@ -588,14 +599,19 @@ extern inline int16_t calc1000toRESX(int16_t x)
|
||||||
|
|
||||||
extern volatile uint16_t g_tmr10ms;
|
extern volatile uint16_t g_tmr10ms;
|
||||||
|
|
||||||
|
#if defined(PCBARM)
|
||||||
|
// This doesn't need protection on this processor
|
||||||
|
#define get_tmr10ms() g_tmr10ms
|
||||||
|
#else
|
||||||
extern inline uint16_t get_tmr10ms()
|
extern inline uint16_t get_tmr10ms()
|
||||||
{
|
{
|
||||||
uint16_t time ;
|
uint16_t time ;
|
||||||
cli();
|
cli();
|
||||||
time = g_tmr10ms ;
|
time = g_tmr10ms ;
|
||||||
sei();
|
sei();
|
||||||
return time ;
|
return time ;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
#define TMR_VAROFS 5
|
#define TMR_VAROFS 5
|
||||||
|
|
||||||
|
@ -683,7 +699,7 @@ extern volatile uint8_t g_rotenc[2];
|
||||||
//audio settungs are external to keep out clutter!
|
//audio settungs are external to keep out clutter!
|
||||||
// TODO english learning for me... what does mean "keep out clutter"?
|
// TODO english learning for me... what does mean "keep out clutter"?
|
||||||
#include "audio.h"
|
#include "audio.h"
|
||||||
#else
|
#elif not defined(PCBARM)
|
||||||
#include "beeper.h"
|
#include "beeper.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -226,5 +226,6 @@ void eeprom_read_block (void *pointer_ram,
|
||||||
size_t size);
|
size_t size);
|
||||||
|
|
||||||
#define wdt_reset() sleep(1)
|
#define wdt_reset() sleep(1)
|
||||||
|
#define board_init()
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -97,7 +97,7 @@
|
||||||
#define TR_FUNCSOUNDS "Warn1 ""Warn2 ""Cheep ""Ring ""SciFi ""Robot ""Chirp ""Tada ""Crickt""Siren ""AlmClk""Ratata""Tick ""Haptc1""Haptc2""Haptc3"
|
#define TR_FUNCSOUNDS "Warn1 ""Warn2 ""Cheep ""Ring ""SciFi ""Robot ""Chirp ""Tada ""Crickt""Siren ""AlmClk""Ratata""Tick ""Haptc1""Haptc2""Haptc3"
|
||||||
|
|
||||||
#define LEN_VTELEMCHNS "\004"
|
#define LEN_VTELEMCHNS "\004"
|
||||||
#define TR_VTELEMCHNS "---\0""A1\0 ""A2\0 ""Alt\0""Rpm\0""Fuel""t1\0 ""t2\0 ""Spd\0""Dist""Cell""Tx\0 ""Rx\0 ""AccX""AccY""AccZ""Hdg\0""a1\0 ""a2\0 ""ALT\0""RPM\0""T1\0 ""T2\0 ""SPD\0""DIST""Tmr1""Tmr2""ACC\0""Time"
|
#define TR_VTELEMCHNS "---\0""Tmr1""Tmr2""A1\0 ""A2\0 ""Tx\0 ""Rx\0 ""Alt\0""Rpm\0""Fuel""T1\0 ""T2\0 ""Spd\0""Dist""Cell""AccX""AccY""AccZ""Hdg\0""A1-\0""A2-\0""Alt-""Alt+""Rpm+""T1+\0""T2+\0""Spd+""Dst+""Acc\0""Time"
|
||||||
|
|
||||||
#define LEN_VTELEMUNIT "\003"
|
#define LEN_VTELEMUNIT "\003"
|
||||||
#define TR_VTELEMUNIT "v\0 ""A\0 ""-\0 ""kts""kmh""M/h""m\0 ""@\0 ""%\0"
|
#define TR_VTELEMUNIT "v\0 ""A\0 ""-\0 ""kts""kmh""M/h""m\0 ""@\0 ""%\0"
|
||||||
|
@ -263,7 +263,6 @@
|
||||||
#define TR_MENUTORESET "[MENU] to reset"
|
#define TR_MENUTORESET "[MENU] to reset"
|
||||||
#define TR_PPM "PPM"
|
#define TR_PPM "PPM"
|
||||||
#define TR_CH "CH"
|
#define TR_CH "CH"
|
||||||
#define TR_TMR "TMR"
|
|
||||||
#define TR_MODEL "MODEL"
|
#define TR_MODEL "MODEL"
|
||||||
#define TR_FP "FP"
|
#define TR_FP "FP"
|
||||||
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
||||||
|
|
|
@ -263,7 +263,6 @@
|
||||||
#define TR_MENUTORESET "[MENU]pour reset"
|
#define TR_MENUTORESET "[MENU]pour reset"
|
||||||
#define TR_PPM "PPM"
|
#define TR_PPM "PPM"
|
||||||
#define TR_CH "CH"
|
#define TR_CH "CH"
|
||||||
#define TR_TMR "TMR"
|
|
||||||
#define TR_MODEL "MODELE"
|
#define TR_MODEL "MODELE"
|
||||||
#define TR_FP "PV"
|
#define TR_FP "PV"
|
||||||
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
||||||
|
|
|
@ -263,7 +263,6 @@
|
||||||
#define TR_MENUTORESET "[MENU] NOLLAR "
|
#define TR_MENUTORESET "[MENU] NOLLAR "
|
||||||
#define TR_PPM "PPM"
|
#define TR_PPM "PPM"
|
||||||
#define TR_CH "KN"
|
#define TR_CH "KN"
|
||||||
#define TR_TMR "TMR"
|
|
||||||
#define TR_MODEL "MODELL"
|
#define TR_MODEL "MODELL"
|
||||||
#define TR_FP "FF"
|
#define TR_FP "FF"
|
||||||
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
#define TR_EEPROMLOWMEM "EEPROM low mem"
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue