mirror of
https://github.com/opentx/opentx.git
synced 2025-07-18 13:55:12 +03:00
parent
6106d1b228
commit
3e84e7930c
20 changed files with 209 additions and 187 deletions
|
@ -320,6 +320,10 @@ if(TRACE_SIMPGMSPACE)
|
||||||
add_definitions(-DTRACE_SIMPGMSPACE)
|
add_definitions(-DTRACE_SIMPGMSPACE)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(EEPROM_VARIANT_NEEDED)
|
||||||
|
add_definitions(-DEEPROM_VARIANT=${EEPROM_VARIANT})
|
||||||
|
endif()
|
||||||
|
|
||||||
set(SRC
|
set(SRC
|
||||||
${SRC}
|
${SRC}
|
||||||
opentx.cpp
|
opentx.cpp
|
||||||
|
@ -413,6 +417,8 @@ set(CMAKE_CXX_LINK_EXECUTABLE "<CMAKE_CXX_COMPILER> <FLAGS> <LINK_FLAGS> <OBJECT
|
||||||
|
|
||||||
use_cxx11() # ensure gnu++11 in CXX_FLAGS with CMake < 3.1
|
use_cxx11() # ensure gnu++11 in CXX_FLAGS with CMake < 3.1
|
||||||
|
|
||||||
|
find_program(BASH bash HINTS "c:/cygwin/bin/" "c:/msys/bin/" DOC "bash shell for firmware size report (eg. c:/cygwin/bin/bash.exe on Windows).")
|
||||||
|
|
||||||
if(NOT MSVC)
|
if(NOT MSVC)
|
||||||
set(OPT s)
|
set(OPT s)
|
||||||
|
|
||||||
|
@ -464,7 +470,6 @@ if(NOT MSVC)
|
||||||
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||||
)
|
)
|
||||||
|
|
||||||
find_program(BASH bash HINTS "c:/cygwin/bin/" DOC "bash shell for firmware size report (eg. c:/cygwin/bin/bash.exe on Windows).")
|
|
||||||
if (BASH)
|
if (BASH)
|
||||||
add_custom_target(firmware-size
|
add_custom_target(firmware-size
|
||||||
COMMAND ${BASH} -kc '${RADIO_DIRECTORY}/util/elf-size-report.sh --mcu=${CPU_TYPE_FULL} ${SIZE_TARGET_MEM_DEFINE} firmware.elf'
|
COMMAND ${BASH} -kc '${RADIO_DIRECTORY}/util/elf-size-report.sh --mcu=${CPU_TYPE_FULL} ${SIZE_TARGET_MEM_DEFINE} firmware.elf'
|
||||||
|
@ -492,14 +497,38 @@ if(NOT MSVC)
|
||||||
include_directories(storage gui/${GUI_DIR})
|
include_directories(storage gui/${GUI_DIR})
|
||||||
|
|
||||||
set(ALLSRC ${PROJECT_BINARY_DIR}/allsrc.cpp)
|
set(ALLSRC ${PROJECT_BINARY_DIR}/allsrc.cpp)
|
||||||
foreach(FILE ${SRC})
|
if (BASH)
|
||||||
file(READ ${FILE} FILE_CONTENT)
|
add_custom_command(
|
||||||
set(ALLSRC_CONTENT "${ALLSRC_CONTENT}# 1 \"${FILE}\"\n${FILE_CONTENT}")
|
OUTPUT ${ALLSRC}
|
||||||
endforeach()
|
COMMAND ${BASH} -kc 'rm -f ${ALLSRC}\; for f in ${SRC}\; do echo "\\# 1 \"$$f\"" >> allsrc.cpp\; cat "$$f" >> ${ALLSRC}\; done'
|
||||||
|
WORKING_DIRECTORY ${RADIO_SRC_DIRECTORY}
|
||||||
file(WRITE ${ALLSRC} "${ALLSRC_CONTENT}")
|
DEPENDS ${SRC}
|
||||||
|
)
|
||||||
|
elseif(WIN32)
|
||||||
|
string(REPLACE "/" "\\" SRC_WIN "${SRC}")
|
||||||
|
string(REPLACE "/" "\\" ALLSRC_WIN "${ALLSRC}")
|
||||||
|
set(ALLSRC_BAT
|
||||||
|
"setlocal EnableDelayedExpansion"
|
||||||
|
"del \"${ALLSRC_WIN}\""
|
||||||
|
"for %%f in (${SRC_WIN}) do ("
|
||||||
|
"set p=\"%%f\""
|
||||||
|
"set p=!p:^\\=/! \necho \# 1 !p! >> \"${ALLSRC_WIN}\""
|
||||||
|
"type %%f >> \"${ALLSRC_WIN}\""
|
||||||
|
")\n"
|
||||||
|
)
|
||||||
|
string(REPLACE ";" "\n" ALLSRC_BAT "${ALLSRC_BAT}")
|
||||||
|
file(WRITE "${ALLSRC}.bat" "${ALLSRC_BAT}")
|
||||||
|
add_custom_command(
|
||||||
|
OUTPUT ${ALLSRC}
|
||||||
|
COMMAND "${ALLSRC}.bat"
|
||||||
|
WORKING_DIRECTORY ${RADIO_SRC_DIRECTORY}
|
||||||
|
DEPENDS ${SRC}
|
||||||
|
)
|
||||||
|
else()
|
||||||
|
set(ALLSRC ${SRC})
|
||||||
|
endif()
|
||||||
add_executable(firmware ${ALLSRC})
|
add_executable(firmware ${ALLSRC})
|
||||||
|
|
||||||
add_dependencies(firmware ${FIRMWARE_DEPENDENCIES})
|
add_dependencies(firmware ${FIRMWARE_DEPENDENCIES})
|
||||||
|
|
||||||
add_custom_command(
|
add_custom_command(
|
||||||
|
@ -534,6 +563,12 @@ if(NOT MSVC)
|
||||||
DEPENDS firmware
|
DEPENDS firmware
|
||||||
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||||
)
|
)
|
||||||
|
else()
|
||||||
|
add_custom_target(flash
|
||||||
|
COMMAND avrdude -c usbasp -p ${AVRDUDE_MCU} -U flash:w:firmware.hex:i
|
||||||
|
DEPENDS firmware
|
||||||
|
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||||
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
PrintTargetReport("firmware")
|
PrintTargetReport("firmware")
|
||||||
|
|
|
@ -410,12 +410,12 @@ void menuModelLogicalSwitches(event_t event)
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
lcdDrawNumber(CSW_3RD_COLUMN, y, cs->v2, LEFT|attr2);
|
lcdDrawNumber(CSW_3RD_COLUMN, y, cs->v2, LEFT|attr2);
|
||||||
#if defined(GVARS)
|
#if defined(CPUARM) && defined(GVARS)
|
||||||
if (v1_val >= MIXSRC_GVAR1) {
|
if (v1_val >= MIXSRC_GVAR1) {
|
||||||
v2_min = -1024; v2_max = +1024;
|
v2_min = -1024; v2_max = +1024;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
#endif // GVARS
|
#endif
|
||||||
{
|
{
|
||||||
v2_min = -LIMIT_EXT_PERCENT; v2_max = +LIMIT_EXT_PERCENT;
|
v2_min = -LIMIT_EXT_PERCENT; v2_max = +LIMIT_EXT_PERCENT;
|
||||||
}
|
}
|
||||||
|
|
|
@ -76,7 +76,7 @@ enum MenuModelOutputsItems {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define LIMITS_CURVE_POS 17*FW+1
|
#define LIMITS_CURVE_POS 17*FW+1
|
||||||
#define LIMITS_MIN_MAX_OFFSET 1000
|
#define LIMITS_MIN_MAX_OFFSET 100
|
||||||
#define CONVERT_US_MIN_MAX(x) ((int16_t(x)*128)/25)
|
#define CONVERT_US_MIN_MAX(x) ((int16_t(x)*128)/25)
|
||||||
#define MIN_MAX_ATTR attr
|
#define MIN_MAX_ATTR attr
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@ void menuRadioTrainer(event_t event)
|
||||||
uint8_t y;
|
uint8_t y;
|
||||||
bool slave = SLAVE_MODE();
|
bool slave = SLAVE_MODE();
|
||||||
|
|
||||||
MENU(STR_MENUTRAINER, menuTabGeneral, MENU_RADIO_TRAINER, (slave ? HEADER_LINE : HEADER_LINE+6), {HEADER_LINE_COLUMNS 2, 2, 2, 2, 0/*, 0*/});
|
MENU(STR_MENUTRAINER, menuTabGeneral, MENU_RADIO_TRAINER, (slave ? HEADER_LINE : HEADER_LINE+6), { HEADER_LINE_COLUMNS 2, 2, 2, 2, 0/*, 0*/ });
|
||||||
|
|
||||||
if (slave) {
|
if (slave) {
|
||||||
lcdDrawText(7*FW, 4*FH, STR_SLAVE);
|
lcdDrawText(7*FW, 4*FH, STR_SLAVE);
|
||||||
|
@ -38,8 +38,8 @@ void menuRadioTrainer(event_t event)
|
||||||
|
|
||||||
y = MENU_HEADER_HEIGHT + 1 + FH;
|
y = MENU_HEADER_HEIGHT + 1 + FH;
|
||||||
|
|
||||||
for (uint8_t i=0; i<NUM_STICKS; i++) {
|
for (uint8_t i=HEADER_LINE; i<HEADER_LINE+NUM_STICKS; i++) {
|
||||||
uint8_t chan = channel_order(i+1);
|
uint8_t chan = channel_order(i+1-HEADER_LINE);
|
||||||
volatile TrainerMix * td = &g_eeGeneral.trainer.mix[chan-1];
|
volatile TrainerMix * td = &g_eeGeneral.trainer.mix[chan-1];
|
||||||
|
|
||||||
drawSource(0, y, MIXSRC_Rud-1+chan, (menuVerticalPosition==i && CURSOR_ON_LINE()) ? INVERS : 0);
|
drawSource(0, y, MIXSRC_Rud-1+chan, (menuVerticalPosition==i && CURSOR_ON_LINE()) ? INVERS : 0);
|
||||||
|
|
|
@ -101,7 +101,7 @@ void menuChannelsView(event_t event)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Gauge
|
// Gauge
|
||||||
// uint16_t lim = (g_model.extendedLimits ? (512 * LIMIT_EXT_PERCENT / 100) : 512) * 2;
|
// uint16_t lim = (g_model.extendedLimits ? (512 * (long)LIMIT_EXT_PERCENT / 100) : 512) * 2;
|
||||||
//#ifdef MIXERS_MONITOR
|
//#ifdef MIXERS_MONITOR
|
||||||
// if (mixersView)
|
// if (mixersView)
|
||||||
// lim = 512 * 2 * 2;
|
// lim = 512 * 2 * 2;
|
||||||
|
|
|
@ -38,7 +38,7 @@
|
||||||
#define VBATTUNIT_X (VBATT_X-1)
|
#define VBATTUNIT_X (VBATT_X-1)
|
||||||
#define VBATTUNIT_Y (3*FH)
|
#define VBATTUNIT_Y (3*FH)
|
||||||
#define REBOOT_X (20*FW-3)
|
#define REBOOT_X (20*FW-3)
|
||||||
#define BAR_HEIGHT (BOX_WIDTH-1)
|
#define BAR_HEIGHT (BOX_WIDTH-1l) // don't remove the l here to force 16bits maths on 9X
|
||||||
#define TRIM_LH_X (LCD_W*1/4+2)
|
#define TRIM_LH_X (LCD_W*1/4+2)
|
||||||
#define TRIM_LV_X 3
|
#define TRIM_LV_X 3
|
||||||
#define TRIM_RV_X (LCD_W-4)
|
#define TRIM_RV_X (LCD_W-4)
|
||||||
|
@ -162,25 +162,26 @@ void displayTrims(uint8_t phase)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void drawTimerWithMode(coord_t x, coord_t y, uint8_t index)
|
FORCEINLINE void drawTimerWithMode(coord_t x, coord_t y, uint8_t index)
|
||||||
{
|
{
|
||||||
// Main timer
|
const TimerData & timer = g_model.timers[index];
|
||||||
if (g_model.timers[index].mode) {
|
if (timer.mode) {
|
||||||
const TimerState & timerState = timersStates[index];
|
const TimerState & timerState = timersStates[index];
|
||||||
LcdFlags att = RIGHT | DBLSIZE | (timerState.val<0 ? BLINK|INVERS : 0);
|
const uint8_t negative = (timerState.val<0 ? BLINK | INVERS : 0);
|
||||||
|
LcdFlags att = RIGHT | DBLSIZE | negative;
|
||||||
drawTimer(x, y, timerState.val, att, att);
|
drawTimer(x, y, timerState.val, att, att);
|
||||||
#if defined(CPUARM)
|
#if defined(CPUARM)
|
||||||
uint8_t xLabel = (timerState.val >= 0 ? x-49 : x-56);
|
uint8_t xLabel = (negative ? x-56 : x-49);
|
||||||
uint8_t len = zlen(g_model.timers[index].name, LEN_TIMER_NAME);
|
uint8_t len = zlen(timer.name, LEN_TIMER_NAME);
|
||||||
if (len > 0) {
|
if (len > 0) {
|
||||||
lcdDrawSizedText(xLabel, y+FH, g_model.timers[index].name, len, RIGHT | ZCHAR);
|
lcdDrawSizedText(xLabel, y+FH, timer.name, len, RIGHT | ZCHAR);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
drawTimerMode(xLabel, y+FH, g_model.timers[index].mode, RIGHT);
|
drawTimerMode(xLabel, y+FH, timer.mode, RIGHT);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
uint8_t xLabel = (timerState.val >= 0 ? x-69 : x-76);
|
uint8_t xLabel = (negative ? x-76 : x-69);
|
||||||
drawTimerMode(xLabel, y+FH, g_model.timers[index].mode);
|
drawTimerMode(xLabel, y+FH, timer.mode);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -495,7 +496,7 @@ void menuMainView(event_t event)
|
||||||
x0 = i<4 ? LCD_W/4+2 : LCD_W*3/4-2;
|
x0 = i<4 ? LCD_W/4+2 : LCD_W*3/4-2;
|
||||||
y0 = 38+(i%4)*5;
|
y0 = 38+(i%4)*5;
|
||||||
|
|
||||||
const uint16_t lim = (g_model.extendedLimits ? 512 * uint8_t(LIMIT_EXT_PERCENT / 100) : 512) * 2;
|
const uint16_t lim = (g_model.extendedLimits ? (512 * (long)LIMIT_EXT_PERCENT / 100) : 512) * 2;
|
||||||
int8_t len = (abs(val) * WBAR2 + lim/2) / lim;
|
int8_t len = (abs(val) * WBAR2 + lim/2) / lim;
|
||||||
|
|
||||||
if (len>WBAR2)
|
if (len>WBAR2)
|
||||||
|
|
|
@ -259,18 +259,6 @@
|
||||||
#define pgm_read_adr(x) *(x)
|
#define pgm_read_adr(x) *(x)
|
||||||
#define cli()
|
#define cli()
|
||||||
#define sei()
|
#define sei()
|
||||||
#else
|
|
||||||
#include <avr/io.h>
|
|
||||||
#include <avr/pgmspace.h>
|
|
||||||
#include "pgmtypes.h"
|
|
||||||
|
|
||||||
#include <avr/eeprom.h>
|
|
||||||
#include <avr/sleep.h>
|
|
||||||
#include <avr/interrupt.h>
|
|
||||||
#define F_CPU 16000000UL // 16 MHz
|
|
||||||
#include <util/delay.h>
|
|
||||||
#define pgm_read_adr(address_short) pgm_read_word(address_short)
|
|
||||||
#include <avr/wdt.h>
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include "debug.h"
|
#include "debug.h"
|
||||||
|
|
|
@ -2,6 +2,7 @@ option(SP22 "SmartieParts 2.2 Backlight support" OFF)
|
||||||
option(PWM_BACKLIGHT "PWM Backlight" OFF)
|
option(PWM_BACKLIGHT "PWM Backlight" OFF)
|
||||||
set(ARCH AVR)
|
set(ARCH AVR)
|
||||||
set(MCU atmega64)
|
set(MCU atmega64)
|
||||||
|
set(AVRDUDE_MCU m64)
|
||||||
string(TOLOWER ${PCB} FLAVOUR)
|
string(TOLOWER ${PCB} FLAVOUR)
|
||||||
set(EEPROM_VARIANT_NEEDED ON)
|
set(EEPROM_VARIANT_NEEDED ON)
|
||||||
set(EEPROM EEPROM_RLC)
|
set(EEPROM EEPROM_RLC)
|
||||||
|
|
|
@ -23,53 +23,51 @@
|
||||||
#if defined(ROTARY_ENCODER_NAVIGATION)
|
#if defined(ROTARY_ENCODER_NAVIGATION)
|
||||||
|
|
||||||
#if defined(TELEMETREZ)
|
#if defined(TELEMETREZ)
|
||||||
uint8_t TrotCount ; // TeZ version
|
uint8_t TrotCount; // TeZ version
|
||||||
uint8_t LastTrotCount ; // TeZ version
|
uint8_t LastTrotCount; // TeZ version
|
||||||
uint8_t RotEncoder ;
|
uint8_t RotEncoder;
|
||||||
#define ROTENC_DOWN() (RotEncoder != 0)
|
|
||||||
#else
|
#else
|
||||||
uint8_t RotPosition ;
|
uint8_t RotPosition;
|
||||||
int8_t LastRotaryValue ;
|
int8_t LastRotaryValue;
|
||||||
int8_t Rotary_diff ;
|
int8_t Rotary_diff;
|
||||||
int8_t RotaryControl ;
|
int8_t RotaryControl;
|
||||||
uint8_t RotEncoder ;
|
uint8_t RotEncoder;
|
||||||
#define ROTENC_DOWN() (RotEncoder & 0x20)
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void rotencPoll()
|
void rotencPoll()
|
||||||
{
|
{
|
||||||
#if defined(TELEMETREZ)
|
#if defined(TELEMETREZ)
|
||||||
if (TrotCount != LastTrotCount) {
|
if (TrotCount != LastTrotCount) {
|
||||||
rotencValue[0] = LastTrotCount = TrotCount ;
|
rotencValue[0] = LastTrotCount = TrotCount;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
// Rotary Encoder polling
|
// Rotary Encoder polling
|
||||||
PORTA = 0 ; // No pullups
|
PORTA = 0; // No pullups
|
||||||
DDRA = 0x1F ; // Top 3 bits input
|
DDRA = 0x1F; // Top 3 bits input
|
||||||
|
|
||||||
asm(" rjmp 1f") ;
|
asm(" rjmp 1f");
|
||||||
asm("1:") ;
|
asm("1:");
|
||||||
|
|
||||||
uint8_t rotary ;
|
uint8_t rotary;
|
||||||
rotary = PINA ;
|
rotary = PINA;
|
||||||
DDRA = 0xFF ; // Back to all outputs
|
DDRA = 0xFF; // Back to all outputs
|
||||||
rotary &= 0xE0 ;
|
rotary &= 0xE0;
|
||||||
|
|
||||||
RotEncoder = rotary ; // just read the lcd pin
|
RotEncoder = rotary; // just read the lcd pin
|
||||||
|
|
||||||
rotary &= 0xDF ;
|
rotary &= 0xDF;
|
||||||
if ( rotary != RotPosition ) {
|
if ( rotary != RotPosition ) {
|
||||||
uint8_t x ;
|
uint8_t x;
|
||||||
x = RotPosition & 0x40 ;
|
x = RotPosition & 0x40;
|
||||||
x <<= 1 ;
|
x <<= 1;
|
||||||
x ^= rotary & 0x80 ;
|
x ^= rotary & 0x80;
|
||||||
if ( x ) {
|
if ( x ) {
|
||||||
rotencValue[0] -= 1 ;
|
rotencValue[0] -= 1;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
rotencValue[0] += 1 ;
|
rotencValue[0] += 1;
|
||||||
}
|
}
|
||||||
RotPosition = rotary ;
|
RotPosition = rotary;
|
||||||
}
|
}
|
||||||
#endif // TELEMETREZ
|
#endif // TELEMETREZ
|
||||||
}
|
}
|
||||||
|
@ -140,14 +138,6 @@ void boardInit()
|
||||||
#endif // !defined(SIMU)
|
#endif // !defined(SIMU)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef SIMU
|
|
||||||
FORCEINLINE
|
|
||||||
#endif
|
|
||||||
uint8_t keyDown()
|
|
||||||
{
|
|
||||||
return ((~PINB) & 0x7E) | ROTENC_DOWN();
|
|
||||||
}
|
|
||||||
|
|
||||||
#if !defined(SIMU) && (defined(TELEMETRY_MOD_14051) || defined(TELEMETRY_MOD_14051_SWAPPED))
|
#if !defined(SIMU) && (defined(TELEMETRY_MOD_14051) || defined(TELEMETRY_MOD_14051_SWAPPED))
|
||||||
uint8_t pf7_digital[MUX_PF7_DIGITAL_MAX - MUX_PF7_DIGITAL_MIN + 1];
|
uint8_t pf7_digital[MUX_PF7_DIGITAL_MAX - MUX_PF7_DIGITAL_MIN + 1];
|
||||||
/**
|
/**
|
||||||
|
@ -306,7 +296,7 @@ void readKeysAndTrims()
|
||||||
|
|
||||||
#if defined(NAVIGATION_STICKS)
|
#if defined(NAVIGATION_STICKS)
|
||||||
if (~PINB & 0x7E) {
|
if (~PINB & 0x7E) {
|
||||||
StickScrollTimer = STICK_SCROLL_TIMEOUT ;
|
StickScrollTimer = STICK_SCROLL_TIMEOUT;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
|
@ -183,7 +183,24 @@ bool checkSlaveMode();
|
||||||
#define INP_G_RF_POW 1
|
#define INP_G_RF_POW 1
|
||||||
#define INP_G_RuddDR 0
|
#define INP_G_RuddDR 0
|
||||||
|
|
||||||
|
// Rotary Encoder driver
|
||||||
|
#if defined(ROTARY_ENCODER_NAVIGATION)
|
||||||
|
extern uint8_t RotEncoder;
|
||||||
|
#if defined(TELEMETREZ)
|
||||||
|
#define ROTENC_DOWN() (RotEncoder != 0)
|
||||||
|
#else
|
||||||
|
#define ROTENC_DOWN() (RotEncoder & 0x20)
|
||||||
|
#endif
|
||||||
|
#else
|
||||||
|
#define ROTENC_DOWN() (0)
|
||||||
|
#endif
|
||||||
|
|
||||||
// Keys driver
|
// Keys driver
|
||||||
|
inline uint8_t keyDown()
|
||||||
|
{
|
||||||
|
return ((~PINB) & 0x7E) | ROTENC_DOWN();
|
||||||
|
}
|
||||||
|
|
||||||
#if defined(TELEMETRY_MOD_14051) || defined(TELEMETRY_MOD_14051_SWAPPED)
|
#if defined(TELEMETRY_MOD_14051) || defined(TELEMETRY_MOD_14051_SWAPPED)
|
||||||
enum MuxInput {
|
enum MuxInput {
|
||||||
MUX_BATT,
|
MUX_BATT,
|
||||||
|
|
|
@ -5,7 +5,7 @@ option(BOLD "Bold font" ON)
|
||||||
option(BATTGRAPH "Battery graph" OFF)
|
option(BATTGRAPH "Battery graph" OFF)
|
||||||
option(HAPTIC "Haptic support" OFF)
|
option(HAPTIC "Haptic support" OFF)
|
||||||
|
|
||||||
set(TELEMETRY_TYPES NONE/MAVLINK/ARDUPILOT/FRSKY/FRSKY_SPORT/TELEMETREZ/NMEA)
|
set(TELEMETRY_TYPES NONE MAVLINK ARDUPILOT FRSKY FRSKY_SPORT TELEMETREZ NMEA)
|
||||||
set(TELEMETRY "NONE" CACHE STRING "Telemetry Type (${TELEMETRY_TYPES})")
|
set(TELEMETRY "NONE" CACHE STRING "Telemetry Type (${TELEMETRY_TYPES})")
|
||||||
set_property(CACHE TELEMETRY PROPERTY STRINGS ${TELEMETRY_TYPES})
|
set_property(CACHE TELEMETRY PROPERTY STRINGS ${TELEMETRY_TYPES})
|
||||||
|
|
||||||
|
@ -129,7 +129,3 @@ if(TELEMETRY STREQUAL FRSKY OR TELEMETRY STREQUAL FRSKY_SPORT OR TELEMETRY STREQ
|
||||||
math(EXPR EEPROM_VARIANT ${EEPROM_VARIANT}+${FRSKY_VARIANT})
|
math(EXPR EEPROM_VARIANT ${EEPROM_VARIANT}+${FRSKY_VARIANT})
|
||||||
endif()
|
endif()
|
||||||
add_definitions(-DTIMERS=2)
|
add_definitions(-DTIMERS=2)
|
||||||
if(EEPROM_VARIANT_NEEDED)
|
|
||||||
add_definitions(-DEEPROM_VARIANT=${EEPROM_VARIANT})
|
|
||||||
endif()
|
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,31 @@
|
||||||
* GNU General Public License for more details.
|
* GNU General Public License for more details.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#if defined(SIMU)
|
||||||
|
extern volatile unsigned char pina, pinb, pinc, pind, pine, pinf, ping, pinh, pinj, pinl;
|
||||||
|
#define PINA ~pina
|
||||||
|
#define PINB ~pinb
|
||||||
|
#define PINC ~pinc
|
||||||
|
#define PIND ~pind
|
||||||
|
#define PINE ~pine
|
||||||
|
#define PINF ~pinf
|
||||||
|
#define PING ~ping
|
||||||
|
#define PINH ~pinh
|
||||||
|
#define PINJ ~pinj
|
||||||
|
#define PINL ~pinl
|
||||||
|
#else
|
||||||
|
#include <avr/io.h>
|
||||||
|
#include <avr/pgmspace.h>
|
||||||
|
#include "pgmtypes.h"
|
||||||
|
#include <avr/eeprom.h>
|
||||||
|
#include <avr/sleep.h>
|
||||||
|
#include <avr/interrupt.h>
|
||||||
|
#define F_CPU 16000000UL // 16 MHz
|
||||||
|
#include <util/delay.h>
|
||||||
|
#define pgm_read_adr(address_short) pgm_read_word(address_short)
|
||||||
|
#include <avr/wdt.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
// ADC driver
|
// ADC driver
|
||||||
#define NUM_POTS 3
|
#define NUM_POTS 3
|
||||||
#define NUM_SLIDERS 0
|
#define NUM_SLIDERS 0
|
||||||
|
@ -220,6 +245,5 @@ uint8_t eepromIsTransferComplete();
|
||||||
#define TLM_USART 0
|
#define TLM_USART 0
|
||||||
#endif
|
#endif
|
||||||
void telemetryPortInit();
|
void telemetryPortInit();
|
||||||
void telemetryPortInit(uint32_t baudrate);
|
void telemetryPortInit(uint8_t baudrate);
|
||||||
void telemetryPortInitFromIndex(uint8_t index);
|
|
||||||
void telemetryTransmitBuffer();
|
void telemetryTransmitBuffer();
|
||||||
|
|
|
@ -23,7 +23,9 @@
|
||||||
|
|
||||||
#if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_MAVLINK)
|
#if defined(TELEMETRY_FRSKY) || defined(TELEMETRY_MAVLINK)
|
||||||
|
|
||||||
enum SERIAL_BAUDS {// KEEP IN SYNC WITH GUI
|
// KEEP IN SYNC WITH GUI
|
||||||
|
enum SERIAL_BAUDS
|
||||||
|
{
|
||||||
BAUD_4800 = 0,
|
BAUD_4800 = 0,
|
||||||
BAUD_9600,
|
BAUD_9600,
|
||||||
BAUD_14400,
|
BAUD_14400,
|
||||||
|
@ -101,7 +103,8 @@ ISR(USART_RX_vect_N(TLM_USART))
|
||||||
UCSRB_N(TLM_USART) |= (1 << RXCIE_N(TLM_USART)); // enable Interrupt
|
UCSRB_N(TLM_USART) |= (1 << RXCIE_N(TLM_USART)); // enable Interrupt
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_4800(void) {
|
static void uart_4800(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 4800
|
#define BAUD 4800
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -114,7 +117,8 @@ static void uart_4800(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_9600(void) {
|
FORCEINLINE void uart_9600(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 9600
|
#define BAUD 9600
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -127,7 +131,8 @@ static void uart_9600(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_14400(void) {
|
static void uart_14400(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 14400
|
#define BAUD 14400
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -140,7 +145,8 @@ static void uart_14400(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_19200(void) {
|
static void uart_19200(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 19200
|
#define BAUD 19200
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -153,7 +159,8 @@ static void uart_19200(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_38400(void) {
|
static void uart_38400(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 38400
|
#define BAUD 38400
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -166,7 +173,8 @@ static void uart_38400(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_57600(void) {
|
static void uart_57600(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 57600
|
#define BAUD 57600
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -179,13 +187,15 @@ static void uart_57600(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_58798(void) {
|
static void uart_58798(void)
|
||||||
|
{
|
||||||
UBRRH_N(TLM_USART) = 0;
|
UBRRH_N(TLM_USART) = 0;
|
||||||
UBRRL_N(TLM_USART) = 0x010;
|
UBRRL_N(TLM_USART) = 0x010;
|
||||||
UCSRA_N(TLM_USART) &= ~(1 << U2X_N(TLM_USART)); // disable double speed operation.
|
UCSRA_N(TLM_USART) &= ~(1 << U2X_N(TLM_USART)); // disable double speed operation.
|
||||||
}
|
}
|
||||||
|
|
||||||
static void uart_76800(void) {
|
static void uart_76800(void)
|
||||||
|
{
|
||||||
#undef BAUD
|
#undef BAUD
|
||||||
#define BAUD 76800
|
#define BAUD 76800
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
|
@ -198,67 +208,31 @@ static void uart_76800(void) {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void telemetryPortInitFromIndex(uint8_t index) {
|
|
||||||
switch (index) {
|
|
||||||
case BAUD_4800:
|
|
||||||
telemetryPortInit(4800);
|
|
||||||
break;
|
|
||||||
case BAUD_9600:
|
|
||||||
telemetryPortInit(9600);
|
|
||||||
break;
|
|
||||||
case BAUD_14400:
|
|
||||||
telemetryPortInit(14400);
|
|
||||||
break;
|
|
||||||
case BAUD_19200:
|
|
||||||
telemetryPortInit(19200);
|
|
||||||
break;
|
|
||||||
case BAUD_38400:
|
|
||||||
telemetryPortInit(38400);
|
|
||||||
break;
|
|
||||||
case BAUD_57600:
|
|
||||||
telemetryPortInit(57600);
|
|
||||||
break;
|
|
||||||
case BAUD_58798:
|
|
||||||
telemetryPortInit(58798);
|
|
||||||
break;
|
|
||||||
case BAUD_76800:
|
|
||||||
telemetryPortInit(76800);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void telemetryPortInit() {
|
|
||||||
telemetryPortInit(9600);
|
|
||||||
}
|
|
||||||
|
|
||||||
void telemetryPortInit(uint32_t baudrate)
|
|
||||||
{
|
|
||||||
#if !defined(SIMU)
|
#if !defined(SIMU)
|
||||||
|
FORCEINLINE void telemetryPortInit(uint8_t baudrate)
|
||||||
|
{
|
||||||
RXD_DDR_N(TLM_USART) &= ~(1 << RXD_DDR_PIN_N(TLM_USART)); // set RXD pin as input
|
RXD_DDR_N(TLM_USART) &= ~(1 << RXD_DDR_PIN_N(TLM_USART)); // set RXD pin as input
|
||||||
RXD_PORT_N(TLM_USART) &= ~(1 << RXD_PORT_PIN_N(TLM_USART)); // disable pullup on RXD pin
|
RXD_PORT_N(TLM_USART) &= ~(1 << RXD_PORT_PIN_N(TLM_USART)); // disable pullup on RXD pin
|
||||||
switch (baudrate) {
|
switch (baudrate) {
|
||||||
case 4800:
|
case BAUD_4800:
|
||||||
uart_4800();
|
uart_4800();
|
||||||
break;
|
break;
|
||||||
case 9600:
|
case BAUD_9600:
|
||||||
uart_9600();
|
uart_9600();
|
||||||
break;
|
break;
|
||||||
case 14400:
|
case BAUD_14400:
|
||||||
uart_14400();
|
uart_14400();
|
||||||
break;
|
break;
|
||||||
case 19200:
|
case BAUD_19200:
|
||||||
uart_19200();
|
uart_19200();
|
||||||
break;
|
break;
|
||||||
case 38400:
|
case BAUD_38400:
|
||||||
uart_38400();
|
uart_38400();
|
||||||
break;
|
break;
|
||||||
case 57600:
|
case BAUD_58798:
|
||||||
uart_57600();
|
|
||||||
break;
|
|
||||||
case 58798:
|
|
||||||
uart_58798();
|
uart_58798();
|
||||||
break;
|
break;
|
||||||
case 76800:
|
case BAUD_76800:
|
||||||
uart_76800();
|
uart_76800();
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
|
@ -270,7 +244,6 @@ void telemetryPortInit(uint32_t baudrate)
|
||||||
UCSRB_N(TLM_USART) = 0 | (0 << RXCIE_N(TLM_USART)) | (0 << TXCIE_N(TLM_USART)) | (0 << UDRIE_N(TLM_USART)) | (0 << RXEN_N(TLM_USART)) | (0 << TXEN_N(TLM_USART)) | (0 << UCSZ2_N(TLM_USART));
|
UCSRB_N(TLM_USART) = 0 | (0 << RXCIE_N(TLM_USART)) | (0 << TXCIE_N(TLM_USART)) | (0 << UDRIE_N(TLM_USART)) | (0 << RXEN_N(TLM_USART)) | (0 << TXEN_N(TLM_USART)) | (0 << UCSZ2_N(TLM_USART));
|
||||||
UCSRC_N(TLM_USART) = 0 | (1 << UCSZ1_N(TLM_USART)) | (1 << UCSZ0_N(TLM_USART));
|
UCSRC_N(TLM_USART) = 0 | (1 << UCSZ1_N(TLM_USART)) | (1 << UCSZ0_N(TLM_USART));
|
||||||
|
|
||||||
|
|
||||||
while (UCSRA_N(TLM_USART) & (1 << RXC_N(TLM_USART))) UDR_N(TLM_USART); // flush receive buffer
|
while (UCSRA_N(TLM_USART) & (1 << RXC_N(TLM_USART))) UDR_N(TLM_USART); // flush receive buffer
|
||||||
|
|
||||||
// These should be running right from power up on a FrSky enabled '9X.
|
// These should be running right from power up on a FrSky enabled '9X.
|
||||||
|
@ -278,12 +251,17 @@ void telemetryPortInit(uint32_t baudrate)
|
||||||
telemetryEnableTx(); // enable FrSky-Telemetry emission
|
telemetryEnableTx(); // enable FrSky-Telemetry emission
|
||||||
#endif
|
#endif
|
||||||
telemetryEnableRx(); // enable FrSky-Telemetry reception
|
telemetryEnableRx(); // enable FrSky-Telemetry reception
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void telemetryTransmitBuffer()
|
void telemetryTransmitBuffer()
|
||||||
{
|
{
|
||||||
UCSRB_N(TLM_USART) |= (1 << UDRIE_N(TLM_USART)); // enable UDRE1 interrupt
|
UCSRB_N(TLM_USART) |= (1 << UDRIE_N(TLM_USART)); // enable UDRE1 interrupt
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void telemetryPortInit()
|
||||||
|
{
|
||||||
|
telemetryPortInit(BAUD_9600);
|
||||||
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
set(ARCH AVR)
|
set(ARCH AVR)
|
||||||
set(MCU atmega2560)
|
set(MCU atmega2560)
|
||||||
|
set(AVRDUDE_MCU m2560)
|
||||||
string(TOLOWER ${PCB} FLAVOUR)
|
string(TOLOWER ${PCB} FLAVOUR)
|
||||||
set(EEPROM EEPROM_RLC)
|
set(EEPROM EEPROM_RLC)
|
||||||
add_definitions(-DEEPROM_VARIANT=0)
|
add_definitions(-DEEPROM_VARIANT=0)
|
||||||
|
|
|
@ -558,6 +558,10 @@ void lcdRefresh()
|
||||||
simuLcdRefresh = true;
|
simuLcdRefresh = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void telemetryPortInit(uint8_t baudrate)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
void telemetryPortInit()
|
void telemetryPortInit()
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
|
@ -250,16 +250,6 @@ extern void rxPdcUsart( void (*pChProcess)(uint8_t x) );
|
||||||
#define DDRE dummyport
|
#define DDRE dummyport
|
||||||
#define DDRF dummyport
|
#define DDRF dummyport
|
||||||
#define DDRG dummyport
|
#define DDRG dummyport
|
||||||
#define PINA ~pina
|
|
||||||
#define PINB ~pinb
|
|
||||||
#define PINC ~pinc
|
|
||||||
#define PIND ~pind
|
|
||||||
#define PINE ~pine
|
|
||||||
#define PINF ~pinf
|
|
||||||
#define PING ~ping
|
|
||||||
#define PINH ~pinh
|
|
||||||
#define PINJ ~pinj
|
|
||||||
#define PINL ~pinl
|
|
||||||
#define EEMEM
|
#define EEMEM
|
||||||
|
|
||||||
#define UCSR0B dummyport
|
#define UCSR0B dummyport
|
||||||
|
@ -354,7 +344,6 @@ extern uint32_t Master_frequency;
|
||||||
#define __enable_irq()
|
#define __enable_irq()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
extern volatile unsigned char pina, pinb, pinc, pind, pine, pinf, ping, pinh, pinj, pinl;
|
|
||||||
extern uint8_t portb, portc, porth, dummyport;
|
extern uint8_t portb, portc, porth, dummyport;
|
||||||
extern uint16_t dummyport16;
|
extern uint16_t dummyport16;
|
||||||
extern uint8_t main_thread_running;
|
extern uint8_t main_thread_running;
|
||||||
|
|
|
@ -315,7 +315,7 @@ void frskySendPacket(uint8_t type, uint8_t value, uint8_t p1, uint8_t p2)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void frskyDSendNextAlarm(void)
|
void frskyDSendNextAlarm(void)
|
||||||
{
|
{
|
||||||
if (frskyTxBufferCount)
|
if (frskyTxBufferCount)
|
||||||
return; // we only have one buffer. If it's in use, then we can't send yet.
|
return; // we only have one buffer. If it's in use, then we can't send yet.
|
||||||
|
|
|
@ -25,10 +25,6 @@
|
||||||
|
|
||||||
#include "telemetry/mavlink.h"
|
#include "telemetry/mavlink.h"
|
||||||
|
|
||||||
#if defined(SIMU)
|
|
||||||
void telemetryPortInitFromIndex(uint8_t index) {}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// this might need to move to the flight software
|
// this might need to move to the flight software
|
||||||
//static
|
//static
|
||||||
mavlink_system_t mavlink_system = { 7, MAV_COMP_ID_MISSIONPLANNER, 0, 0, 0, 0 };
|
mavlink_system_t mavlink_system = { 7, MAV_COMP_ID_MISSIONPLANNER, 0, 0, 0, 0 };
|
||||||
|
@ -78,10 +74,11 @@ void MAVLINK_reset(uint8_t warm_reset) {
|
||||||
}
|
}
|
||||||
|
|
||||||
//! \brief initalize mavlink extension
|
//! \brief initalize mavlink extension
|
||||||
void MAVLINK_Init(void) {
|
void MAVLINK_Init(void)
|
||||||
|
{
|
||||||
mav_statustext[0] = 0;
|
mav_statustext[0] = 0;
|
||||||
MAVLINK_reset(0);
|
MAVLINK_reset(0);
|
||||||
telemetryPortInitFromIndex(g_eeGeneral.mavbaud);
|
telemetryPortInit(g_eeGeneral.mavbaud);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*! \brief Status log message
|
/*! \brief Status log message
|
||||||
|
@ -583,7 +580,7 @@ void telemetryWakeup() {
|
||||||
|
|
||||||
if (mav_heartbeat == -30) {
|
if (mav_heartbeat == -30) {
|
||||||
MAVLINK_reset(1);
|
MAVLINK_reset(1);
|
||||||
telemetryPortInitFromIndex(g_eeGeneral.mavbaud);
|
telemetryPortInit(g_eeGeneral.mavbaud);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
last_time = tmr10ms;
|
last_time = tmr10ms;
|
||||||
|
|
|
@ -320,5 +320,7 @@ inline uint8_t getPrecisMavlinParamsValue(uint8_t idx) {
|
||||||
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode);
|
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
void telemetryPortInit(uint8_t baudrate);
|
||||||
|
|
||||||
#endif // _MAVLINK_H_
|
#endif // _MAVLINK_H_
|
||||||
|
|
||||||
|
|
|
@ -4,7 +4,6 @@ FROM debian:jessie
|
||||||
|
|
||||||
RUN apt-get update && \
|
RUN apt-get update && \
|
||||||
apt-get install -y \
|
apt-get install -y \
|
||||||
avr-libc \
|
|
||||||
build-essential \
|
build-essential \
|
||||||
cmake \
|
cmake \
|
||||||
gcc \
|
gcc \
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue