1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-13 03:19:53 +03:00

Add T16 support

Added support for:
- T16 radio
- internal multi-protocol module.
- flashing internal and external multi-protocol modules.

Many thx to:
- goebish, for the hardware support.
- qba667, for the original patches for the internal multi-protocol support (UI and serial com).
This commit is contained in:
Raphael Coeffic 2019-08-30 15:49:19 +02:00 committed by Raphael Coeffic
parent 4a618f1317
commit 6b12010473
98 changed files with 4585 additions and 481 deletions

View file

@ -33,6 +33,7 @@ env:
# - FLAVOR=X9D # - FLAVOR=X9D
# - FLAVOR=X9D+ # - FLAVOR=X9D+
# - FLAVOR=X9E # - FLAVOR=X9E
- FLAVOR=T16
- FLAVOR=HORUS - FLAVOR=HORUS
# - FLAVOR=X10 # - FLAVOR=X10
# - FLAVOR=X12Sr10 # - FLAVOR=X12Sr10

View file

@ -347,6 +347,8 @@ elseif(PCB STREQUAL X9D+ AND PCBREV STREQUAL 2019)
set(FLAVOUR x9d+2019) set(FLAVOUR x9d+2019)
elseif(PCB STREQUAL X10 AND PCBREV STREQUAL EXPRESS) elseif(PCB STREQUAL X10 AND PCBREV STREQUAL EXPRESS)
set(FLAVOUR x10express) set(FLAVOUR x10express)
elseif(PCB STREQUAL X10 AND PCBREV STREQUAL T16)
set(FLAVOUR t16)
else() else()
string(TOLOWER ${PCB} FLAVOUR) string(TOLOWER ${PCB} FLAVOUR)
endif() endif()

View file

@ -170,6 +170,17 @@
<file>images/simulator/JumperT12/JumperT12-x.png</file> <file>images/simulator/JumperT12/JumperT12-x.png</file>
<file>images/simulator/JumperT12/JumperT12-center.png</file> <file>images/simulator/JumperT12/JumperT12-center.png</file>
<file>images/simulator/JumperT12/JumperT12-top.png</file> <file>images/simulator/JumperT12/JumperT12-top.png</file>
<file>images/simulator/JumperT16/left.png</file>
<file>images/simulator/JumperT16/right.png</file>
<file>images/simulator/JumperT16/top.png</file>
<file>images/simulator/JumperT16/bottom.png</file>
<file>images/simulator/JumperT16/left_mdl.png</file>
<file>images/simulator/JumperT16/left_page.png</file>
<file>images/simulator/JumperT16/left_scrnsht.png</file>
<file>images/simulator/JumperT16/left_sys.png</file>
<file>images/simulator/JumperT16/left_tele.png</file>
<file>images/simulator/JumperT16/right_ent.png</file>
<file>images/simulator/JumperT16/right_rtn.png</file>
<file>images/wizard/ailerons.png</file> <file>images/wizard/ailerons.png</file>
<file>images/wizard/airbrakes.png</file> <file>images/wizard/airbrakes.png</file>
<file>images/wizard/elevons.png</file> <file>images/wizard/elevons.png</file>

View file

@ -89,6 +89,8 @@ uint32_t Boards::getFourCC(Type board)
return 0; return 0;
case BOARD_JUMPER_T12: case BOARD_JUMPER_T12:
return 0x3D78746F; return 0x3D78746F;
case BOARD_JUMPER_T16:
return 0x3F78746F;
case BOARD_UNKNOWN: case BOARD_UNKNOWN:
break; break;
} }
@ -127,6 +129,7 @@ int Boards::getEEpromSize(Board::Type board)
case BOARD_HORUS_X12S: case BOARD_HORUS_X12S:
case BOARD_X10: case BOARD_X10:
case BOARD_X10_EXPRESS: case BOARD_X10_EXPRESS:
case BOARD_JUMPER_T16:
return 0; return 0;
} }
@ -162,6 +165,7 @@ int Boards::getFlashSize(Type board)
case BOARD_HORUS_X12S: case BOARD_HORUS_X12S:
case BOARD_X10: case BOARD_X10:
case BOARD_X10_EXPRESS: case BOARD_X10_EXPRESS:
case BOARD_JUMPER_T16:
return FSIZE_HORUS; return FSIZE_HORUS;
case BOARD_UNKNOWN: case BOARD_UNKNOWN:
return FSIZE_MAX; return FSIZE_MAX;
@ -277,7 +281,7 @@ int Boards::getCapability(Board::Type board, Board::Capability capability)
return 2; return 2;
else if (IS_TARANIS_X9E(board)) else if (IS_TARANIS_X9E(board))
return 4; return 4;
else if (IS_HORUS_X10(board)) else if (IS_HORUS_X10(board) || IS_JUMPER_T16(board))
return 5; return 5;
else if (IS_HORUS_X12S(board)) else if (IS_HORUS_X12S(board))
return 3; return 3;
@ -293,7 +297,7 @@ int Boards::getCapability(Board::Type board, Board::Capability capability)
case Sliders: case Sliders:
if (IS_HORUS_X12S(board) || IS_TARANIS_X9E(board)) if (IS_HORUS_X12S(board) || IS_TARANIS_X9E(board))
return 4; return 4;
else if (IS_TARANIS_X9D(board) || IS_HORUS_X10(board)) else if (IS_TARANIS_X9D(board) || IS_HORUS_X10(board) || IS_JUMPER_T16(board))
return 2; return 2;
else else
return 0; return 0;
@ -458,7 +462,7 @@ QString Boards::getAnalogInputName(Board::Type board, int index)
if (index < DIM(pots)) if (index < DIM(pots))
return pots[index]; return pots[index];
} }
else if (IS_HORUS_X10(board)) { else if (IS_HORUS_X10(board) || IS_JUMPER_T16(board)) {
const QString pots[] = { const QString pots[] = {
"S1", "S1",
"6P", "6P",
@ -523,6 +527,8 @@ QString Boards::getBoardName(Board::Type board)
return "Horus X10/X10S"; return "Horus X10/X10S";
case BOARD_X10_EXPRESS: case BOARD_X10_EXPRESS:
return "Horus X10 Express"; return "Horus X10 Express";
case BOARD_JUMPER_T16:
return "Jumper T16";
default: default:
return tr("Unknown"); return tr("Unknown");
} }

View file

@ -52,9 +52,10 @@ namespace Board {
BOARD_TARANIS_X9LITE, BOARD_TARANIS_X9LITE,
BOARD_TARANIS_X9LITES, BOARD_TARANIS_X9LITES,
BOARD_JUMPER_T12, BOARD_JUMPER_T12,
BOARD_JUMPER_T16,
}; };
constexpr int BOARD_TYPE_MAX = BOARD_JUMPER_T12 ; constexpr int BOARD_TYPE_MAX = BOARD_JUMPER_T16 ;
enum PotType enum PotType
{ {
@ -223,6 +224,11 @@ inline bool IS_JUMPER_T12(Board::Type board)
return board == Board::BOARD_JUMPER_T12; return board == Board::BOARD_JUMPER_T12;
} }
inline bool IS_JUMPER_T16(Board::Type board)
{
return board == Board::BOARD_JUMPER_T16;
}
inline bool IS_TARANIS_XLITE(Board::Type board) inline bool IS_TARANIS_XLITE(Board::Type board)
{ {
return board == Board::BOARD_TARANIS_XLITE || board == Board::BOARD_TARANIS_XLITES; return board == Board::BOARD_TARANIS_XLITE || board == Board::BOARD_TARANIS_XLITES;
@ -285,7 +291,7 @@ inline bool IS_HORUS_X12S(Board::Type board)
inline bool IS_HORUS(Board::Type board) inline bool IS_HORUS(Board::Type board)
{ {
return IS_HORUS_X12S(board) || IS_HORUS_X10(board); return IS_HORUS_X12S(board) || IS_HORUS_X10(board) || IS_JUMPER_T16(board);
} }
inline bool IS_HORUS_OR_TARANIS(Board::Type board) inline bool IS_HORUS_OR_TARANIS(Board::Type board)
@ -310,7 +316,7 @@ inline bool HAS_LARGE_LCD(Board::Type board)
inline bool HAS_EXTERNAL_ANTENNA(Board::Type board) inline bool HAS_EXTERNAL_ANTENNA(Board::Type board)
{ {
return (IS_HORUS(board) && board != Board::BOARD_X10_EXPRESS) || (IS_TARANIS_XLITE(board) && !IS_TARANIS_XLITES(board)); return (board == Board::BOARD_X10 || board == Board::BOARD_HORUS_X12S || (IS_TARANIS_XLITE(board) && !IS_TARANIS_XLITES(board)));
} }
#endif // _BOARDS_H_ #endif // _BOARDS_H_

View file

@ -88,7 +88,7 @@ GeneralSettings::GeneralSettings()
vBatMin = -5; //8,5V vBatMin = -5; //8,5V
vBatMax = -5; //11,5V vBatMax = -5; //11,5V
} }
else if (IS_TARANIS_XLITE(board) || IS_HORUS_X10(board)) { else if (IS_TARANIS_XLITE(board) || IS_HORUS_X10(board) || IS_JUMPER_T16(board)) {
// Lipo 2S // Lipo 2S
vBatWarn = 66; vBatWarn = 66;
vBatMin = -23; // 6.7V vBatMin = -23; // 6.7V
@ -267,7 +267,7 @@ void GeneralSettings::setDefaultControlTypes(Board::Type board)
sliderConfig[2] = Board::SLIDER_WITH_DETENT; sliderConfig[2] = Board::SLIDER_WITH_DETENT;
sliderConfig[3] = Board::SLIDER_WITH_DETENT; sliderConfig[3] = Board::SLIDER_WITH_DETENT;
} }
else if (IS_TARANIS_X9(board) || IS_HORUS_X10(board)) { else if (IS_TARANIS_X9(board) || IS_HORUS_X10(board) || IS_JUMPER_T16(board)) {
sliderConfig[0] = Board::SLIDER_WITH_DETENT; sliderConfig[0] = Board::SLIDER_WITH_DETENT;
sliderConfig[1] = Board::SLIDER_WITH_DETENT; sliderConfig[1] = Board::SLIDER_WITH_DETENT;
} }

View file

@ -68,6 +68,8 @@ const char * OpenTxEepromInterface::getName()
return "OpenTX for Gruvin9x board / 9X"; return "OpenTX for Gruvin9x board / 9X";
case BOARD_JUMPER_T12: case BOARD_JUMPER_T12:
return "OpenTX for Jumper T12"; return "OpenTX for Jumper T12";
case BOARD_JUMPER_T16:
return "OpenTX for Jumper T16";
case BOARD_TARANIS_X9D: case BOARD_TARANIS_X9D:
return "OpenTX for FrSky Taranis X9D"; return "OpenTX for FrSky Taranis X9D";
case BOARD_TARANIS_X9DP: case BOARD_TARANIS_X9DP:
@ -770,18 +772,20 @@ bool OpenTxFirmware::isAvailable(PulsesProtocol proto, int port)
return true; return true;
case PULSES_PXX_XJT_X16: case PULSES_PXX_XJT_X16:
case PULSES_PXX_XJT_LR12: case PULSES_PXX_XJT_LR12:
return !(IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board)); return !(IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board) || IS_JUMPER_T16(board));
case PULSES_PXX_XJT_D8: case PULSES_PXX_XJT_D8:
return !(IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board) || id.contains("eu")); return !(IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board) || IS_JUMPER_T16(board) || id.contains("eu"));
case PULSES_PPM: case PULSES_PPM:
return id.contains("internalppm"); return id.contains("internalppm");
case PULSES_ACCESS_ISRM: case PULSES_ACCESS_ISRM:
case PULSES_ACCST_ISRM_D16: case PULSES_ACCST_ISRM_D16:
return IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board) || board == BOARD_TARANIS_X9DP_2019 || board == BOARD_X10_EXPRESS || (IS_HORUS(board) && id.contains("internalaccess")); return IS_TARANIS_XLITES(board) || IS_TARANIS_X9LITE(board) || board == BOARD_TARANIS_X9DP_2019 || board == BOARD_X10_EXPRESS || (IS_HORUS(board) && id.contains("internalaccess"));
case PULSES_MULTIMODULE:
return id.contains("internalmulti");
default: default:
return false; return false;
} }
case 1: case 1:
switch (proto) { switch (proto) {
case PULSES_OFF: case PULSES_OFF:
@ -1291,6 +1295,12 @@ void registerOpenTxFirmwares()
addOpenTxFontOptions(firmware); addOpenTxFontOptions(firmware);
registerOpenTxFirmware(firmware); registerOpenTxFirmware(firmware);
/* Jumper T16 board */
firmware = new OpenTxFirmware("opentx-t16", Firmware::tr("Jumper T16 / T16+"), BOARD_JUMPER_T16);
addOpenTxFrskyOptions(firmware);
firmware->addOption("internalmulti", Firmware::tr("Support for MULTI internal module"));
registerOpenTxFirmware(firmware);
/* 9XR-Pro */ /* 9XR-Pro */
firmware = new OpenTxFirmware("opentx-9xrpro", Firmware::tr("Turnigy 9XR-PRO"), BOARD_9XRPRO); firmware = new OpenTxFirmware("opentx-9xrpro", Firmware::tr("Turnigy 9XR-PRO"), BOARD_9XRPRO);
addOpenTxArm9xOptions(firmware, false); addOpenTxArm9xOptions(firmware, false);

File diff suppressed because it is too large Load diff

After

Width:  |  Height:  |  Size: 104 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 250 B

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 12 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 11 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 11 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 439 B

View file

@ -12,6 +12,7 @@ set(simulation_SRCS
simulateduiwidgetX10.cpp simulateduiwidgetX10.cpp
simulateduiwidgetX12.cpp simulateduiwidgetX12.cpp
simulateduiwidgetJumperT12.cpp simulateduiwidgetJumperT12.cpp
simulateduiwidgetJumperT16.cpp
simulatorinterface.cpp simulatorinterface.cpp
simulatormainwindow.cpp simulatormainwindow.cpp
simulatorstartupdialog.cpp simulatorstartupdialog.cpp
@ -34,6 +35,7 @@ set(simulation_UIS
simulateduiwidgetX10.ui simulateduiwidgetX10.ui
simulateduiwidgetX12.ui simulateduiwidgetX12.ui
simulateduiwidgetJumperT12.ui simulateduiwidgetJumperT12.ui
simulateduiwidgetJumperT16.ui
simulatormainwindow.ui simulatormainwindow.ui
simulatorstartupdialog.ui simulatorstartupdialog.ui
simulatorwidget.ui simulatorwidget.ui

View file

@ -111,6 +111,7 @@ namespace Ui {
class SimulatedUIWidgetX10; class SimulatedUIWidgetX10;
class SimulatedUIWidgetX12; class SimulatedUIWidgetX12;
class SimulatedUIWidgetJumperT12; class SimulatedUIWidgetJumperT12;
class SimulatedUIWidgetJumperT16;
} }
class SimulatedUIWidget9X: public SimulatedUIWidget class SimulatedUIWidget9X: public SimulatedUIWidget
@ -225,4 +226,17 @@ class SimulatedUIWidgetJumperT12: public SimulatedUIWidget
Ui::SimulatedUIWidgetJumperT12 * ui; Ui::SimulatedUIWidgetJumperT12 * ui;
}; };
class SimulatedUIWidgetJumperT16: public SimulatedUIWidget
{
Q_OBJECT
public:
explicit SimulatedUIWidgetJumperT16(SimulatorInterface * simulator, QWidget * parent = nullptr);
virtual ~SimulatedUIWidgetJumperT16();
private:
Ui::SimulatedUIWidgetJumperT16 * ui;
};
#endif // SIMULATEDUIWIDGET_H #endif // SIMULATEDUIWIDGET_H

View file

@ -8,7 +8,6 @@ SimulatedUIWidgetJumperT12::SimulatedUIWidgetJumperT12(SimulatorInterface *simul
ui(new Ui::SimulatedUIWidgetJumperT12) ui(new Ui::SimulatedUIWidgetJumperT12)
{ {
RadioUiAction * act; RadioUiAction * act;
QPolygon polygon;
ui->setupUi(this); ui->setupUi(this);

View file

@ -0,0 +1,69 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* 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.
*/
// NOTE: RadioUiAction(NUMBER,...): NUMBER relates to enum EnumKeys in the specific board.h
#include "simulateduiwidget.h"
#include "ui_simulateduiwidgetJumperT16.h"
SimulatedUIWidgetJumperT16::SimulatedUIWidgetJumperT16(SimulatorInterface *simulator, QWidget * parent):
SimulatedUIWidget(simulator, parent),
ui(new Ui::SimulatedUIWidgetJumperT16)
{
RadioUiAction * act;
ui->setupUi(this);
// add actions in order of appearance on the help menu
act = new RadioUiAction(3, QList<int>() << Qt::Key_Up, SIMU_STR_HLP_KEY_UP, SIMU_STR_HLP_ACT_MDL);
addRadioWidget(ui->leftbuttons->addArea(QRect(10, 70, 80, 35), "JumperT16/left_mdl.png", act));
act = new RadioUiAction(6, QList<int>() << Qt::Key_Left, SIMU_STR_HLP_KEY_LFT, SIMU_STR_HLP_ACT_SYS);
addRadioWidget(ui->leftbuttons->addArea(QRect(10, 10, 80, 35), "JumperT16/left_sys.png", act));
act = new RadioUiAction(5, QList<int>() << Qt::Key_Right, SIMU_STR_HLP_KEY_RGT, SIMU_STR_HLP_ACT_TELE);
addRadioWidget(ui->leftbuttons->addArea(QRect(10, 190, 80, 35), "JumperT16/left_tele.png", act));
act = new RadioUiAction(4, QList<int>() << Qt::Key_Down << Qt::Key_Delete << Qt::Key_Escape << Qt::Key_Backspace,
SIMU_STR_HLP_KEY_DN % "<br>" % SIMU_STR_HLP_KEYS_EXIT, SIMU_STR_HLP_ACT_RTN);
addRadioWidget(ui->rightbuttons->addArea(QRect(35, 10, 80, 35), "JumperT16/right_rtn.png", act));
act = new RadioUiAction(1, QList<int>() << Qt::Key_PageDown, SIMU_STR_HLP_KEY_PGDN, SIMU_STR_HLP_ACT_PGDN);
addRadioWidget(ui->leftbuttons->addArea(QRect(10, 131, 80, 35), "JumperT16/left_page.png", act));
m_scrollUpAction = new RadioUiAction(-1, QList<int>() << Qt::Key_Minus, SIMU_STR_HLP_KEY_MIN % "|" % SIMU_STR_HLP_MOUSE_UP, SIMU_STR_HLP_ACT_ROT_LFT);
m_scrollDnAction = new RadioUiAction(-1, QList<int>() << Qt::Key_Plus << Qt::Key_Equal, SIMU_STR_HLP_KEY_PLS % "|" % SIMU_STR_HLP_MOUSE_DN, SIMU_STR_HLP_ACT_ROT_RGT);
connectScrollActions();
m_mouseMidClickAction = new RadioUiAction(2, QList<int>() << Qt::Key_Enter << Qt::Key_Return, SIMU_STR_HLP_KEYS_ACTIVATE, SIMU_STR_HLP_ACT_ROT_DN);
addRadioWidget(ui->rightbuttons->addArea(QRect(45, 70, 100, 160), "JumperT16/right_ent.png", m_mouseMidClickAction));
addRadioWidget(ui->leftbuttons->addArea(QRect(10, 252, 30, 30), "JumperT16/left_scrnsht.png", m_screenshotAction));
m_backlightColors << QColor(47, 123, 227);
setLcd(ui->lcd);
}
SimulatedUIWidgetJumperT16::~SimulatedUIWidgetJumperT16()
{
delete ui;
}

View file

@ -0,0 +1,197 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>SimulatedUIWidgetJumperT16</class>
<widget class="QWidget" name="SimulatedUIWidgetJumperT16">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>825</width>
<height>292</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>825</width>
<height>292</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>825</width>
<height>292</height>
</size>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="margin">
<number>0</number>
</property>
<property name="spacing">
<number>0</number>
</property>
<item row="0" column="2" rowspan="3">
<widget class="ButtonsWidget" name="rightbuttons" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>174</width>
<height>292</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>174</width>
<height>292</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background:url(:/images/simulator/JumperT16/right.png)</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="LcdWidget" name="lcd" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>480</width>
<height>272</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>480</width>
<height>272</height>
</size>
</property>
<property name="font">
<font>
<pointsize>5</pointsize>
</font>
</property>
</widget>
</item>
<item row="0" column="0" rowspan="3">
<widget class="ButtonsWidget" name="leftbuttons" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>174</width>
<height>292</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>174</width>
<height>292</height>
</size>
</property>
<property name="mouseTracking">
<bool>true</bool>
</property>
<property name="styleSheet">
<string notr="true">background:url(:/images/simulator/JumperT16/left.png);</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QWidget" name="top" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>480</width>
<height>10</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>480</width>
<height>10</height>
</size>
</property>
<property name="font">
<font>
<pointsize>5</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true">background:url(:/images/simulator/JumperT16/top.png)</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QWidget" name="bottom" native="true">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>480</width>
<height>10</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>480</width>
<height>10</height>
</size>
</property>
<property name="font">
<font>
<pointsize>5</pointsize>
<kerning>false</kerning>
</font>
</property>
<property name="styleSheet">
<string notr="true">background:url(:/images/simulator/JumperT16/bottom.png)</string>
</property>
</widget>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>LcdWidget</class>
<extends>QWidget</extends>
<header>lcdwidget.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>ButtonsWidget</class>
<extends>QWidget</extends>
<header>buttonswidget.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>

View file

@ -86,7 +86,7 @@ void SimulatorStartupDialog::changeEvent(QEvent *e)
// FIXME : need a better way to check for this // FIXME : need a better way to check for this
bool SimulatorStartupDialog::usesCategorizedStorage(const QString & name) bool SimulatorStartupDialog::usesCategorizedStorage(const QString & name)
{ {
return name.contains(QRegExp("(x12|x10|horus)", Qt::CaseInsensitive)); return name.contains(QRegExp("(x12|x10|horus|t16)", Qt::CaseInsensitive));
} }
bool SimulatorStartupDialog::usesCategorizedStorage() bool SimulatorStartupDialog::usesCategorizedStorage()

View file

@ -87,7 +87,10 @@ SimulatorWidget::SimulatorWidget(QWidget * parent, SimulatorInterface * simulato
break; break;
case Board::BOARD_JUMPER_T12: case Board::BOARD_JUMPER_T12:
radioUiWidget = new SimulatedUIWidgetJumperT12(simulator, this); radioUiWidget = new SimulatedUIWidgetJumperT12(simulator, this);
break; break;
case Board::BOARD_JUMPER_T16:
radioUiWidget = new SimulatedUIWidgetJumperT16(simulator, this);
break;
default: default:
radioUiWidget = new SimulatedUIWidget9X(simulator, this); radioUiWidget = new SimulatedUIWidget9X(simulator, this);
break; break;

View file

@ -27,6 +27,7 @@ build:
cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=X9E ../ ; make libsimulator cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=X9E ../ ; make libsimulator
cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=X12S ../ ; make libsimulator cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=X12S ../ ; make libsimulator
cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=JUMPERT12 ../ ; make libsimulator cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=JUMPERT12 ../ ; make libsimulator
cd $(BUILDDIR); cmake $(COMMON_OPTIONS) $(STM32_OPTIONS) -DPCB=JUMPERT16 ../ ; make libsimulator
make -C $(BUILDDIR) companion22 simulator22 make -C $(BUILDDIR) companion22 simulator22

Binary file not shown.

After

Width:  |  Height:  |  Size: 161 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 161 KiB

View file

@ -208,20 +208,28 @@ enum TrainerMode {
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
#define TRAINER_MODE_MAX() TRAINER_MODE_SLAVE_BLUETOOTH #define TRAINER_MODE_MAX() TRAINER_MODE_SLAVE_BLUETOOTH
#elif defined(RADIO_T16)
#define TRAINER_MODE_MAX() TRAINER_MODE_SLAVE
#elif defined(PCBX7) || defined(PCBXLITE) #elif defined(PCBX7) || defined(PCBXLITE)
#define TRAINER_MODE_MAX() TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE #define TRAINER_MODE_MAX() TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE
#else #else
#define TRAINER_MODE_MAX() HAS_WIRELESS_TRAINER_HARDWARE() ? TRAINER_MODE_MASTER_BATTERY_COMPARTMENT : TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE #define TRAINER_MODE_MAX() HAS_WIRELESS_TRAINER_HARDWARE() ? TRAINER_MODE_MASTER_BATTERY_COMPARTMENT : TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE
#endif #endif
#if defined(PCBTARANIS) || defined(PCBHORUS) #if defined(HARDWARE_INTERNAL_MODULE)
#define IS_INTERNAL_MODULE_ENABLED() (g_model.moduleData[INTERNAL_MODULE].type != MODULE_TYPE_NONE) #define IS_INTERNAL_MODULE_ENABLED() (g_model.moduleData[INTERNAL_MODULE].type != MODULE_TYPE_NONE)
#elif defined(PCBSKY9X) #else
#define IS_INTERNAL_MODULE_ENABLED() (false) #define IS_INTERNAL_MODULE_ENABLED() (false)
#endif #endif
#define IS_EXTERNAL_MODULE_ENABLED() (g_model.moduleData[EXTERNAL_MODULE].type != MODULE_TYPE_NONE) #define IS_EXTERNAL_MODULE_ENABLED() (g_model.moduleData[EXTERNAL_MODULE].type != MODULE_TYPE_NONE)
#if defined(HARDWARE_INTERNAL_MODULE)
#define IS_MODULE_ENABLED(moduleIdx) (moduleIdx==INTERNAL_MODULE ? IS_INTERNAL_MODULE_ENABLED() : moduleIdx==EXTERNAL_MODULE ? IS_EXTERNAL_MODULE_ENABLED() : false)
#else
#define IS_MODULE_ENABLED(moduleIdx) (moduleIdx==EXTERNAL_MODULE ? IS_EXTERNAL_MODULE_ENABLED() : false)
#endif
enum UartModes { enum UartModes {
#if defined(CLI) || defined(DEBUG) #if defined(CLI) || defined(DEBUG)
UART_MODE_DEBUG, UART_MODE_DEBUG,

View file

@ -77,11 +77,6 @@ class Fifo
return (next == ridx); return (next == ridx);
} }
void flush()
{
while (!isEmpty());
}
uint32_t size() const uint32_t size() const
{ {
return (N + widx - ridx) & (N - 1); return (N + widx - ridx) & (N - 1);

View file

@ -176,9 +176,6 @@ enum MenuModelSetupItems {
#define SW_WARN_ROWS uint8_t(NAVIGATION_LINE_BY_LINE|(getSwitchWarningsCount()-1)), uint8_t(getSwitchWarningsCount() > MAX_SWITCH_PER_LINE ? TITLE_ROW : HIDDEN_ROW) #define SW_WARN_ROWS uint8_t(NAVIGATION_LINE_BY_LINE|(getSwitchWarningsCount()-1)), uint8_t(getSwitchWarningsCount() > MAX_SWITCH_PER_LINE ? TITLE_ROW : HIDDEN_ROW)
#endif #endif
#define IF_INTERNAL_MODULE_ON(x) (IS_INTERNAL_MODULE_ENABLED()? (uint8_t)(x) : HIDDEN_ROW)
#define IF_EXTERNAL_MODULE_ON(x) (IS_EXTERNAL_MODULE_ENABLED()? (uint8_t)(x) : HIDDEN_ROW)
#if defined(INTERNAL_MODULE_PXX1) #if defined(INTERNAL_MODULE_PXX1)
#define INTERNAL_MODULE_TYPE_ROWS ((isModuleXJT(INTERNAL_MODULE) || isModulePXX2(INTERNAL_MODULE)) ? (uint8_t)1 : (uint8_t)0) // Module type + RF protocols #define INTERNAL_MODULE_TYPE_ROWS ((isModuleXJT(INTERNAL_MODULE) || isModulePXX2(INTERNAL_MODULE)) ? (uint8_t)1 : (uint8_t)0) // Module type + RF protocols
#else #else
@ -191,8 +188,6 @@ enum MenuModelSetupItems {
#define OUTPUT_TYPE_ROW #define OUTPUT_TYPE_ROW
#endif #endif
#define PORT_CHANNELS_ROWS(x) (x==EXTERNAL_MODULE ? EXTERNAL_MODULE_CHANNELS_ROWS : 0)
inline uint8_t EXTERNAL_MODULE_TYPE_ROW() inline uint8_t EXTERNAL_MODULE_TYPE_ROW()
{ {
if (isModuleXJT(EXTERNAL_MODULE) || isModuleR9MNonAccess(EXTERNAL_MODULE) || isModuleDSM2(EXTERNAL_MODULE)) if (isModuleXJT(EXTERNAL_MODULE) || isModuleR9MNonAccess(EXTERNAL_MODULE) || isModuleDSM2(EXTERNAL_MODULE))
@ -283,7 +278,7 @@ void onBluetoothConnectMenu(const char * result)
#define INTERNAL_MODULE_ROWS \ #define INTERNAL_MODULE_ROWS \
LABEL(InternalModule), \ LABEL(InternalModule), \
INTERNAL_MODULE_TYPE_ROWS, \ INTERNAL_MODULE_TYPE_ROWS, \
INTERNAL_MODULE_CHANNELS_ROWS, \ MODULE_CHANNELS_ROWS(INTERNAL_MODULE), \
IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(isModuleRxNumAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1)), \ IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(isModuleRxNumAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1)), \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* RxNum */ \ IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* RxNum */ \
EXTERNAL_ANTENNA_ROW \ EXTERNAL_ANTENNA_ROW \
@ -334,13 +329,13 @@ void menuModelSetup(event_t event)
LABEL(ExternalModule), LABEL(ExternalModule),
EXTERNAL_MODULE_TYPE_ROW(), EXTERNAL_MODULE_TYPE_ROW(),
MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE) MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_POWER_ROW, MODULE_POWER_ROW(EXTERNAL_MODULE),
MULTIMODULE_STATUS_ROWS MULTIMODULE_STATUS_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_CHANNELS_ROWS, MODULE_CHANNELS_ROWS(EXTERNAL_MODULE),
IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, EXTERNAL_MODULE_BIND_ROWS), // line reused for PPM: PPM settings IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, MODULE_BIND_ROWS(EXTERNAL_MODULE)), // line reused for PPM: PPM settings
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum
IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, EXTERNAL_MODULE_OPTION_ROW), IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, MODULE_OPTION_ROW(EXTERNAL_MODULE)),
MULTIMODULE_MODULE_ROWS MULTIMODULE_MODULE_ROWS(EXTERNAL_MODULE)
FAILSAFE_ROWS(EXTERNAL_MODULE), FAILSAFE_ROWS(EXTERNAL_MODULE),
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons
IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options
@ -355,13 +350,13 @@ void menuModelSetup(event_t event)
LABEL(ExternalModule), LABEL(ExternalModule),
EXTERNAL_MODULE_TYPE_ROW(), EXTERNAL_MODULE_TYPE_ROW(),
MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE) MULTIMODULE_SUBTYPE_ROWS(EXTERNAL_MODULE)
MULTIMODULE_STATUS_ROWS MULTIMODULE_STATUS_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_CHANNELS_ROWS, MODULE_CHANNELS_ROWS(EXTERNAL_MODULE),
EXTERNAL_MODULE_BIND_ROWS, MODULE_BIND_ROWS(EXTERNAL_MODULE),
OUTPUT_TYPE_ROW OUTPUT_TYPE_ROW
EXTERNAL_MODULE_OPTION_ROW, MODULE_OPTION_ROW(EXTERNAL_MODULE),
MULTIMODULE_MODULE_ROWS MULTIMODULE_MODULE_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_POWER_ROW, MODULE_POWER_ROW(EXTERNAL_MODULE),
FAILSAFE_ROWS(EXTERNAL_MODULE), FAILSAFE_ROWS(EXTERNAL_MODULE),
EXTRA_MODULE_ROWS EXTRA_MODULE_ROWS
TRAINER_ROWS TRAINER_ROWS
@ -1066,26 +1061,24 @@ void menuModelSetup(event_t event)
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k); uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
ModuleData & moduleData = g_model.moduleData[moduleIdx]; ModuleData & moduleData = g_model.moduleData[moduleIdx];
lcdDrawTextAlignedLeft(y, STR_CHANNELRANGE); lcdDrawTextAlignedLeft(y, STR_CHANNELRANGE);
if ((int8_t)PORT_CHANNELS_ROWS(moduleIdx) >= 0) { lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, STR_CH, menuHorizontalPosition==0 ? attr : 0);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, STR_CH, menuHorizontalPosition==0 ? attr : 0); lcdDrawNumber(lcdLastRightPos, y, moduleData.channelsStart+1, LEFT | (menuHorizontalPosition==0 ? attr : 0));
lcdDrawNumber(lcdLastRightPos, y, moduleData.channelsStart+1, LEFT | (menuHorizontalPosition==0 ? attr : 0)); lcdDrawChar(lcdLastRightPos, y, '-');
lcdDrawChar(lcdLastRightPos, y, '-'); lcdDrawNumber(lcdLastRightPos + FW+1, y, moduleData.channelsStart+sentModuleChannels(moduleIdx), LEFT | (menuHorizontalPosition==1 ? attr : 0));
lcdDrawNumber(lcdLastRightPos + FW+1, y, moduleData.channelsStart+sentModuleChannels(moduleIdx), LEFT | (menuHorizontalPosition==1 ? attr : 0)); const char * delay = getModuleDelay(moduleIdx);
const char * delay = getModuleDelay(moduleIdx); if (delay)
if (delay) lcdDrawText(lcdLastRightPos+4, y, delay, SMLSIZE);
lcdDrawText(lcdLastRightPos+4, y, delay, SMLSIZE); if (attr && s_editMode > 0) {
if (attr && s_editMode > 0) { switch (menuHorizontalPosition) {
switch (menuHorizontalPosition) { case 0:
case 0: CHECK_INCDEC_MODELVAR_ZERO(event, moduleData.channelsStart, 32-8-moduleData.channelsCount);
CHECK_INCDEC_MODELVAR_ZERO(event, moduleData.channelsStart, 32-8-moduleData.channelsCount); break;
break; case 1:
case 1: CHECK_INCDEC_MODELVAR_CHECK(event, moduleData.channelsCount, -4, min<int8_t>(maxModuleChannels_M8(moduleIdx), 32-8-moduleData.channelsStart), moduleData.type == MODULE_TYPE_ISRM_PXX2 ? isPxx2IsrmChannelsCountAllowed : nullptr);
CHECK_INCDEC_MODELVAR_CHECK(event, moduleData.channelsCount, -4, min<int8_t>(maxModuleChannels_M8(moduleIdx), 32-8-moduleData.channelsStart), moduleData.type == MODULE_TYPE_ISRM_PXX2 ? isPxx2IsrmChannelsCountAllowed : nullptr); if (checkIncDec_Ret && moduleData.type == MODULE_TYPE_PPM) {
if (checkIncDec_Ret && moduleData.type == MODULE_TYPE_PPM) { setDefaultPpmFrameLength(moduleIdx);
setDefaultPpmFrameLength(moduleIdx); }
} break;
break;
}
} }
} }
break; break;
@ -1270,8 +1263,8 @@ void menuModelSetup(event_t event)
lcdDrawText(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, l_posHorz==2 ? attr : 0); lcdDrawText(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, l_posHorz==2 ? attr : 0);
uint8_t newFlag = 0; uint8_t newFlag = 0;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (multiBindStatus == MULTI_BIND_FINISHED) { if (getMultiBindStatus(moduleIdx) == MULTI_BIND_FINISHED) {
multiBindStatus = MULTI_NORMAL_OPERATION; setMultiBindStatus(moduleIdx, MULTI_NORMAL_OPERATION);
s_editMode = 0; s_editMode = 0;
} }
#endif #endif
@ -1321,7 +1314,7 @@ void menuModelSetup(event_t event)
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (newFlag == MODULE_MODE_BIND) { if (newFlag == MODULE_MODE_BIND) {
multiBindStatus = MULTI_BIND_INITIATED; setMultiBindStatus(moduleIdx, MULTI_BIND_INITIATED);
} }
#endif #endif
@ -1532,7 +1525,7 @@ void menuModelSetup(event_t event)
lcdDrawTextAlignedLeft(y, STR_MODULE_STATUS); lcdDrawTextAlignedLeft(y, STR_MODULE_STATUS);
char statusText[64]; char statusText[64];
multiModuleStatus.getStatusString(statusText); getMultiModuleStatus(EXTERNAL_MODULE).getStatusString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
} }
@ -1541,7 +1534,7 @@ void menuModelSetup(event_t event)
lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC); lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
char statusText[64]; char statusText[64];
multiSyncStatus.getRefreshString(statusText); getMultiSyncStatus(EXTERNAL_MODULE).getRefreshString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
} }

View file

@ -230,8 +230,6 @@ void editTimerCountdown(int timerIdx, coord_t y, LcdFlags attr, event_t event)
#define INTERNAL_MODULE_TYPE_ROWS (0) // Module type + RF protocols #define INTERNAL_MODULE_TYPE_ROWS (0) // Module type + RF protocols
#endif #endif
#define PORT_CHANNELS_ROWS(x) (x==INTERNAL_MODULE ? INTERNAL_MODULE_CHANNELS_ROWS : (x==EXTERNAL_MODULE ? EXTERNAL_MODULE_CHANNELS_ROWS : 1))
#define TRAINER_CHANNELS_ROW (IS_SLAVE_TRAINER() ? (uint8_t)1 : HIDDEN_ROW) #define TRAINER_CHANNELS_ROW (IS_SLAVE_TRAINER() ? (uint8_t)1 : HIDDEN_ROW)
#define TRAINER_PPM_PARAMS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)2 : HIDDEN_ROW) #define TRAINER_PPM_PARAMS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)2 : HIDDEN_ROW)
#define TRAINER_BLUETOOTH_M_ROW ((bluetooth.distantAddr[0] == '\0' || bluetooth.state == BLUETOOTH_STATE_CONNECTED) ? (uint8_t)0 : (uint8_t)1) #define TRAINER_BLUETOOTH_M_ROW ((bluetooth.distantAddr[0] == '\0' || bluetooth.state == BLUETOOTH_STATE_CONNECTED) ? (uint8_t)0 : (uint8_t)1)
@ -352,7 +350,7 @@ void menuModelSetup(event_t event)
LABEL(InternalModule), LABEL(InternalModule),
INTERNAL_MODULE_TYPE_ROWS, INTERNAL_MODULE_TYPE_ROWS,
INTERNAL_MODULE_CHANNELS_ROWS, MODULE_CHANNELS_ROWS(INTERNAL_MODULE),
IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(isModuleFailsafeAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1)), IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(isModuleFailsafeAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1)),
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), // RxNum for ACCESS IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), // RxNum for ACCESS
IF_INTERNAL_MODULE_ON(FAILSAFE_ROWS(INTERNAL_MODULE)), // Failsafe IF_INTERNAL_MODULE_ON(FAILSAFE_ROWS(INTERNAL_MODULE)), // Failsafe
@ -364,13 +362,13 @@ void menuModelSetup(event_t event)
LABEL(ExternalModule), LABEL(ExternalModule),
EXTERNAL_MODULE_TYPE_ROW(), EXTERNAL_MODULE_TYPE_ROW(),
MULTIMODULE_STATUS_ROWS MULTIMODULE_STATUS_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_CHANNELS_ROWS, MODULE_CHANNELS_ROWS(EXTERNAL_MODULE),
IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, EXTERNAL_MODULE_BIND_ROWS), IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, MODULE_BIND_ROWS(EXTERNAL_MODULE)),
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum for ACCESS IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum for ACCESS
IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, EXTERNAL_MODULE_OPTION_ROW), IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, MODULE_OPTION_ROW(EXTERNAL_MODULE)),
MULTIMODULE_MODULE_ROWS MULTIMODULE_MODULE_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_POWER_ROW, MODULE_POWER_ROW(EXTERNAL_MODULE),
FAILSAFE_ROWS(EXTERNAL_MODULE), FAILSAFE_ROWS(EXTERNAL_MODULE),
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons
IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options
@ -1019,7 +1017,7 @@ void menuModelSetup(event_t event)
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k); uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
ModuleData & moduleData = g_model.moduleData[moduleIdx]; ModuleData & moduleData = g_model.moduleData[moduleIdx];
lcdDrawTextAlignedLeft(y, STR_CHANNELRANGE); lcdDrawTextAlignedLeft(y, STR_CHANNELRANGE);
if ((int8_t)PORT_CHANNELS_ROWS(moduleIdx) >= 0) { if ((int8_t)MODULE_CHANNELS_ROWS(moduleIdx) >= 0) {
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, STR_CH, menuHorizontalPosition==0 ? attr : 0); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, STR_CH, menuHorizontalPosition==0 ? attr : 0);
lcdDrawNumber(lcdLastRightPos, y, moduleData.channelsStart+1, LEFT | (menuHorizontalPosition==0 ? attr : 0)); lcdDrawNumber(lcdLastRightPos, y, moduleData.channelsStart+1, LEFT | (menuHorizontalPosition==0 ? attr : 0));
lcdDrawChar(lcdLastRightPos, y, '-'); lcdDrawChar(lcdLastRightPos, y, '-');
@ -1144,8 +1142,8 @@ void menuModelSetup(event_t event)
lcdDrawText(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, l_posHorz==2 ? attr : 0); lcdDrawText(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, l_posHorz==2 ? attr : 0);
uint8_t newFlag = 0; uint8_t newFlag = 0;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (multiBindStatus == MULTI_BIND_FINISHED) { if (getMultiBindStatus(moduleIdx) == MULTI_BIND_FINISHED) {
multiBindStatus = MULTI_NORMAL_OPERATION; setMultiBindStatus(moduleIdx, MULTI_NORMAL_OPERATION);
s_editMode = 0; s_editMode = 0;
} }
#endif #endif
@ -1178,7 +1176,7 @@ void menuModelSetup(event_t event)
moduleState[moduleIdx].mode = newFlag; moduleState[moduleIdx].mode = newFlag;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (newFlag == MODULE_MODE_BIND) if (newFlag == MODULE_MODE_BIND)
multiBindStatus = MULTI_BIND_INITIATED; setMultiBindStatus(moduleIdx, MULTI_BIND_INITIATED);
#endif #endif
} }
} }
@ -1317,7 +1315,7 @@ void menuModelSetup(event_t event)
{ {
lcdDrawTextAlignedLeft(y, STR_MODULE_STATUS); lcdDrawTextAlignedLeft(y, STR_MODULE_STATUS);
char statusText[64]; char statusText[64];
multiModuleStatus.getStatusString(statusText); getMultiModuleStatus(EXTERNAL_MODULE).getStatusString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
} }
@ -1326,7 +1324,7 @@ void menuModelSetup(event_t event)
{ {
lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC); lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
char statusText[64]; char statusText[64];
multiSyncStatus.getRefreshString(statusText); getMultiSyncStatus(EXTERNAL_MODULE).getRefreshString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
} }

View file

@ -66,22 +66,31 @@ enum MenuModelSetupItems {
#if defined(PXX2) #if defined(PXX2)
ITEM_MODEL_SETUP_REGISTRATION_ID, ITEM_MODEL_SETUP_REGISTRATION_ID,
#endif #endif
#if defined(HARDWARE_INTERNAL_MODULE)
ITEM_MODEL_SETUP_INTERNAL_MODULE_LABEL, ITEM_MODEL_SETUP_INTERNAL_MODULE_LABEL,
ITEM_MODEL_SETUP_INTERNAL_MODULE_TYPE, ITEM_MODEL_SETUP_INTERNAL_MODULE_TYPE,
#if defined(INTERNAL_MODULE_MULTI)
ITEM_MODEL_SETUP_INTERNAL_MODULE_STATUS,
ITEM_MODEL_SETUP_INTERNAL_MODULE_SYNCSTATUS,
#endif
ITEM_MODEL_SETUP_INTERNAL_MODULE_CHANNELS, ITEM_MODEL_SETUP_INTERNAL_MODULE_CHANNELS,
ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND, ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_MODEL_NUM, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_MODEL_NUM,
ITEM_MODEL_SETUP_INTERNAL_MODULE_OPTIONS,
#if defined(INTERNAL_MODULE_MULTI)
ITEM_MODEL_SETUP_INTERNAL_MODULE_AUTOBIND,
#endif
#if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA) #if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA)
ITEM_MODEL_SETUP_INTERNAL_MODULE_ANTENNA, ITEM_MODEL_SETUP_INTERNAL_MODULE_ANTENNA,
#endif #endif
ITEM_MODEL_SETUP_INTERNAL_MODULE_POWER,
ITEM_MODEL_SETUP_INTERNAL_MODULE_FAILSAFE, ITEM_MODEL_SETUP_INTERNAL_MODULE_FAILSAFE,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_REGISTER_RANGE, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_REGISTER_RANGE,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_OPTIONS, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_OPTIONS,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_2, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_2,
ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_3, ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_3,
#endif
ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL, ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL,
ITEM_MODEL_SETUP_EXTERNAL_MODULE_TYPE, ITEM_MODEL_SETUP_EXTERNAL_MODULE_TYPE,
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
@ -122,7 +131,11 @@ enum MenuModelSetupItems {
#define MODEL_SETUP_SLIDPOT_SPACING 45 #define MODEL_SETUP_SLIDPOT_SPACING 45
#define CURRENT_MODULE_EDITED(k) (k >= ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL ? EXTERNAL_MODULE : INTERNAL_MODULE) #define CURRENT_MODULE_EDITED(k) (k >= ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL ? EXTERNAL_MODULE : INTERNAL_MODULE)
#if defined(HARDWARE_INTERNAL_MODULE)
#define CURRENT_RECEIVER_EDITED(k) (k - (k >= ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL ? ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_1 : ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1)) #define CURRENT_RECEIVER_EDITED(k) (k - (k >= ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL ? ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_1 : ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1))
#else
#define CURRENT_RECEIVER_EDITED(k) (EXTERNAL_MODULE)
#endif
#include "common/stdlcd/model_setup_pxx1.cpp" #include "common/stdlcd/model_setup_pxx1.cpp"
@ -468,19 +481,15 @@ int getSwitchWarningsCount()
#define IF_ACCESS_MODULE_RF(module, xxx) (isModuleRFAccess(module) ? (uint8_t)(xxx) : HIDDEN_ROW) #define IF_ACCESS_MODULE_RF(module, xxx) (isModuleRFAccess(module) ? (uint8_t)(xxx) : HIDDEN_ROW)
#define IF_NOT_ACCESS_MODULE_RF(module, xxx) (isModuleRFAccess(module) ? HIDDEN_ROW : (uint8_t)(xxx)) #define IF_NOT_ACCESS_MODULE_RF(module, xxx) (isModuleRFAccess(module) ? HIDDEN_ROW : (uint8_t)(xxx))
#define INTERNAL_MODULE_TYPE_ROWS ((isModuleXJT(INTERNAL_MODULE) || isModulePXX2(INTERNAL_MODULE)) ? (uint8_t)1 : (uint8_t)0) // Module type + RF protocols
#define MODULE_CHANNELS_ROWS(x) (x==INTERNAL_MODULE ? INTERNAL_MODULE_CHANNELS_ROWS : (x==EXTERNAL_MODULE ? EXTERNAL_MODULE_CHANNELS_ROWS : 1))
#define TIMER_ROWS(x) NAVIGATION_LINE_BY_LINE|1, 0, 0, 0, g_model.timers[x].countdownBeep != COUNTDOWN_SILENT ? (uint8_t)1 : (uint8_t)0 #define TIMER_ROWS(x) NAVIGATION_LINE_BY_LINE|1, 0, 0, 0, g_model.timers[x].countdownBeep != COUNTDOWN_SILENT ? (uint8_t)1 : (uint8_t)0
inline uint8_t EXTERNAL_MODULE_TYPE_ROW() inline uint8_t MODULE_TYPE_ROWS(int moduleIdx)
{ {
if (isModuleXJT(EXTERNAL_MODULE) || isModuleR9MNonAccess(EXTERNAL_MODULE) || isModuleDSM2(EXTERNAL_MODULE)) if (isModuleXJT(moduleIdx) || isModuleR9MNonAccess(moduleIdx) || isModuleDSM2(moduleIdx) || isModulePXX2(moduleIdx))
return 1; return 1;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
else if (isModuleMultimodule(EXTERNAL_MODULE)) { else if (isModuleMultimodule(moduleIdx)) {
return 1 + MULTIMODULE_RFPROTO_COLUMNS(EXTERNAL_MODULE); return 1 + MULTIMODULE_RFPROTO_COLUMNS(moduleIdx);
} }
#endif #endif
else else
@ -495,6 +504,7 @@ inline uint8_t EXTERNAL_MODULE_TYPE_ROW()
#define TIMERS_ROWS TIMER_ROWS(0), TIMER_ROWS(1), TIMER_ROWS(2) #define TIMERS_ROWS TIMER_ROWS(0), TIMER_ROWS(1), TIMER_ROWS(2)
#endif #endif
#if defined(BLUETOOTH)
#define TRAINER_CHANNELS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)1 : (g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH ? (uint8_t)0 : HIDDEN_ROW)) #define TRAINER_CHANNELS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)1 : (g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH ? (uint8_t)0 : HIDDEN_ROW))
#define TRAINER_PPM_PARAMS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)2 : HIDDEN_ROW) #define TRAINER_PPM_PARAMS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)2 : HIDDEN_ROW)
#define IF_BT_TRAINER_ON(x) (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER ? (uint8_t)(x) : HIDDEN_ROW) #define IF_BT_TRAINER_ON(x) (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER ? (uint8_t)(x) : HIDDEN_ROW)
@ -502,6 +512,11 @@ inline uint8_t EXTERNAL_MODULE_TYPE_ROW()
#define TRAINER_BLUETOOTH_S_ROW (bluetooth.distantAddr[0] == '\0' ? HIDDEN_ROW : LABEL()) #define TRAINER_BLUETOOTH_S_ROW (bluetooth.distantAddr[0] == '\0' ? HIDDEN_ROW : LABEL())
#define TRAINER_BLUETOOTH_ROW (g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH ? TRAINER_BLUETOOTH_M_ROW : (g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH ? TRAINER_BLUETOOTH_S_ROW : HIDDEN_ROW)) #define TRAINER_BLUETOOTH_ROW (g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH ? TRAINER_BLUETOOTH_M_ROW : (g_model.trainerData.mode == TRAINER_MODE_SLAVE_BLUETOOTH ? TRAINER_BLUETOOTH_S_ROW : HIDDEN_ROW))
#define TRAINER_ROWS LABEL(Trainer), 0, IF_BT_TRAINER_ON(TRAINER_BLUETOOTH_ROW), TRAINER_CHANNELS_ROW, TRAINER_PPM_PARAMS_ROW #define TRAINER_ROWS LABEL(Trainer), 0, IF_BT_TRAINER_ON(TRAINER_BLUETOOTH_ROW), TRAINER_CHANNELS_ROW, TRAINER_PPM_PARAMS_ROW
#else
#define TRAINER_CHANNELS_ROW (g_model.trainerData.mode == TRAINER_MODE_SLAVE ? (uint8_t)1 : HIDDEN_ROW)
#define TRAINER_PPM_PARAMS_ROW (HIDDEN_ROW)
#define TRAINER_ROWS LABEL(Trainer), 0, TRAINER_CHANNELS_ROW, TRAINER_PPM_PARAMS_ROW
#endif
#if defined(PXX2) #if defined(PXX2)
#define REGISTRATION_ID_ROWS uint8_t((isDefaultModelRegistrationID() || (warningText && popupFunc == runPopupRegister)) ? HIDDEN_ROW : READONLY_ROW), #define REGISTRATION_ID_ROWS uint8_t((isDefaultModelRegistrationID() || (warningText && popupFunc == runPopupRegister)) ? HIDDEN_ROW : READONLY_ROW),
@ -527,11 +542,35 @@ void onModelAntennaSwitchConfirm(const char * result)
#define EXTERNAL_ANTENNA_ROW #define EXTERNAL_ANTENNA_ROW
#endif #endif
#if defined(HARDWARE_INTERNAL_MODULE)
#define INTERNAL_MODULE_ROWS \
LABEL(InternalModule), \
MODULE_TYPE_ROWS(INTERNAL_MODULE), \
MULTIMODULE_STATUS_ROWS(INTERNAL_MODULE) \
MODULE_CHANNELS_ROWS(INTERNAL_MODULE), \
IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(IF_INTERNAL_MODULE_ON(isModuleRxNumAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1))), \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), \
MODULE_OPTION_ROW(INTERNAL_MODULE), \
MULTIMODULE_MODULE_ROWS(INTERNAL_MODULE) \
EXTERNAL_ANTENNA_ROW \
MODULE_POWER_ROW(INTERNAL_MODULE), \
IF_INTERNAL_MODULE_ON(FAILSAFE_ROWS(INTERNAL_MODULE)), \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 1), /* Range check and Register buttons */ \
IF_PXX2_MODULE(INTERNAL_MODULE, 0), /* Module options */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 1 */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 2 */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 3 */
#else
#define INTERNAL_MODULE_ROWS
#endif
bool menuModelSetup(event_t event) bool menuModelSetup(event_t event)
{ {
bool CURSOR_ON_CELL = (menuHorizontalPosition >= 0); bool CURSOR_ON_CELL = (menuHorizontalPosition >= 0);
int8_t old_editMode = s_editMode; int8_t old_editMode = s_editMode;
MENU(STR_MENUSETUP, MODEL_ICONS, menuTabModel, MENU_MODEL_SETUP, ITEM_MODEL_SETUP_MAX, MENU(STR_MENUSETUP, MODEL_ICONS, menuTabModel, MENU_MODEL_SETUP, ITEM_MODEL_SETUP_MAX,
{ 0, // Model name { 0, // Model name
0, // Model bitmap 0, // Model bitmap
@ -559,28 +598,17 @@ bool menuModelSetup(event_t event)
REGISTRATION_ID_ROWS REGISTRATION_ID_ROWS
LABEL(InternalModule), INTERNAL_MODULE_ROWS
INTERNAL_MODULE_TYPE_ROWS,
INTERNAL_MODULE_CHANNELS_ROWS,
IF_NOT_ACCESS_MODULE_RF(INTERNAL_MODULE, IF_INTERNAL_MODULE_ON(IF_INTERNAL_MODULE_ON(isModuleRxNumAvailable(INTERNAL_MODULE) ? (uint8_t)2 : (uint8_t)1))),
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), // RxNum
EXTERNAL_ANTENNA_ROW
IF_INTERNAL_MODULE_ON(FAILSAFE_ROWS(INTERNAL_MODULE)),
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 1), /* Range check and Register buttons */ \
IF_PXX2_MODULE(INTERNAL_MODULE, 0), /* Module options */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 1 */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 2 */ \
IF_ACCESS_MODULE_RF(INTERNAL_MODULE, 0), /* Receiver 3 */
LABEL(ExternalModule), LABEL(ExternalModule),
EXTERNAL_MODULE_TYPE_ROW(), MODULE_TYPE_ROWS(EXTERNAL_MODULE),
MULTIMODULE_STATUS_ROWS MULTIMODULE_STATUS_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_CHANNELS_ROWS, MODULE_CHANNELS_ROWS(EXTERNAL_MODULE),
IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, EXTERNAL_MODULE_BIND_ROWS), IF_NOT_ACCESS_MODULE_RF(EXTERNAL_MODULE, MODULE_BIND_ROWS(EXTERNAL_MODULE)),
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum for ACCESS IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 0), // RxNum for ACCESS
IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, EXTERNAL_MODULE_OPTION_ROW), IF_NOT_PXX2_MODULE(EXTERNAL_MODULE, MODULE_OPTION_ROW(EXTERNAL_MODULE)),
MULTIMODULE_MODULE_ROWS MULTIMODULE_MODULE_ROWS(EXTERNAL_MODULE)
EXTERNAL_MODULE_POWER_ROW, MODULE_POWER_ROW(EXTERNAL_MODULE),
FAILSAFE_ROWS(EXTERNAL_MODULE), FAILSAFE_ROWS(EXTERNAL_MODULE),
IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons IF_ACCESS_MODULE_RF(EXTERNAL_MODULE, 1), // Range check and Register buttons
IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options IF_PXX2_MODULE(EXTERNAL_MODULE, 0), // Module options
@ -613,7 +641,7 @@ bool menuModelSetup(event_t event)
if (mstate_tab[j] == HIDDEN_ROW) if (mstate_tab[j] == HIDDEN_ROW)
k++; k++;
} }
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
LcdFlags blink = ((s_editMode>0) ? BLINK|INVERS : INVERS); LcdFlags blink = ((s_editMode>0) ? BLINK|INVERS : INVERS);
LcdFlags attr = (sub == k ? blink : 0); LcdFlags attr = (sub == k ? blink : 0);
@ -945,10 +973,12 @@ bool menuModelSetup(event_t event)
if (attr) g_model.noGlobalFunctions = !checkIncDecModel(event, !g_model.noGlobalFunctions, 0, 1); if (attr) g_model.noGlobalFunctions = !checkIncDecModel(event, !g_model.noGlobalFunctions, 0, 1);
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_LABEL: case ITEM_MODEL_SETUP_INTERNAL_MODULE_LABEL:
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_INTERNALRF); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_INTERNALRF);
break; break;
#if !defined(INTERNAL_MODULE_MULTI)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_TYPE: case ITEM_MODEL_SETUP_INTERNAL_MODULE_TYPE:
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE);
lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_INTERNAL_MODULE_PROTOCOLS, g_model.moduleData[INTERNAL_MODULE].type, menuHorizontalPosition==0 ? attr : 0); lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_INTERNAL_MODULE_PROTOCOLS, g_model.moduleData[INTERNAL_MODULE].type, menuHorizontalPosition==0 ? attr : 0);
@ -976,6 +1006,7 @@ bool menuModelSetup(event_t event)
} }
} }
break; break;
#endif
#if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA) #if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_ANTENNA: case ITEM_MODEL_SETUP_INTERNAL_MODULE_ANTENNA:
@ -993,111 +1024,124 @@ bool menuModelSetup(event_t event)
} }
break; break;
#endif #endif
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_LABEL:
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_EXTERNALRF); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_EXTERNALRF);
break; break;
#if defined(INTERNAL_MODULE_MULTI)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_TYPE:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_TYPE: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_TYPE:
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE);
lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_EXTERNAL_MODULE_PROTOCOLS, g_model.moduleData[EXTERNAL_MODULE].type, menuHorizontalPosition==0 ? attr : 0); lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_EXTERNAL_MODULE_PROTOCOLS, g_model.moduleData[moduleIdx].type, menuHorizontalPosition==0 ? attr : 0);
if (isModuleXJT(EXTERNAL_MODULE)) if (isModuleXJT(moduleIdx))
lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_XJT_ACCST_RF_PROTOCOLS, 1+g_model.moduleData[EXTERNAL_MODULE].subType, (menuHorizontalPosition==1 ? attr : 0)); lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_XJT_ACCST_RF_PROTOCOLS, 1+g_model.moduleData[moduleIdx].subType, (menuHorizontalPosition==1 ? attr : 0));
else if (isModuleDSM2(EXTERNAL_MODULE)) else if (isModuleDSM2(moduleIdx))
lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_DSM_PROTOCOLS, g_model.moduleData[EXTERNAL_MODULE].rfProtocol, (menuHorizontalPosition==1 ? attr : 0)); lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_DSM_PROTOCOLS, g_model.moduleData[moduleIdx].rfProtocol, (menuHorizontalPosition==1 ? attr : 0));
else if (isModuleR9MNonAccess(EXTERNAL_MODULE)) else if (isModuleR9MNonAccess(moduleIdx))
lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_R9M_REGION, g_model.moduleData[EXTERNAL_MODULE].subType, (menuHorizontalPosition==1 ? attr : 0)); lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_R9M_REGION, g_model.moduleData[moduleIdx].subType, (menuHorizontalPosition==1 ? attr : 0));
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
else if (isModuleMultimodule(EXTERNAL_MODULE)) { else if (isModuleMultimodule(moduleIdx)) {
int multi_rfProto = g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false); int multi_rfProto = g_model.moduleData[moduleIdx].getMultiProtocol(false);
if (g_model.moduleData[EXTERNAL_MODULE].multi.customProto) { if (g_model.moduleData[moduleIdx].multi.customProto) {
lcdDrawText(MODEL_SETUP_3RD_COLUMN, y, STR_MULTI_CUSTOM, menuHorizontalPosition == 1 ? attr : 0); lcdDrawText(MODEL_SETUP_3RD_COLUMN, y, STR_MULTI_CUSTOM, menuHorizontalPosition == 1 ? attr : 0);
lcdDrawNumber(MODEL_SETUP_4TH_COLUMN, y, multi_rfProto, menuHorizontalPosition==2 ? attr : 0, 2); lcdDrawNumber(MODEL_SETUP_4TH_COLUMN, y, multi_rfProto, menuHorizontalPosition==2 ? attr : 0, 2);
lcdDrawNumber(MODEL_SETUP_4TH_COLUMN + MODEL_SETUP_BIND_OFS, y, g_model.moduleData[EXTERNAL_MODULE].subType, menuHorizontalPosition==3 ? attr : 0, 2); lcdDrawNumber(MODEL_SETUP_4TH_COLUMN + MODEL_SETUP_BIND_OFS, y, g_model.moduleData[moduleIdx].subType, menuHorizontalPosition==3 ? attr : 0, 2);
} }
else { else {
const mm_protocol_definition * pdef = getMultiProtocolDefinition(multi_rfProto); const mm_protocol_definition * pdef = getMultiProtocolDefinition(multi_rfProto);
lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_MULTI_PROTOCOLS, multi_rfProto, menuHorizontalPosition == 1 ? attr : 0); lcdDrawTextAtIndex(MODEL_SETUP_3RD_COLUMN, y, STR_MULTI_PROTOCOLS, multi_rfProto, menuHorizontalPosition == 1 ? attr : 0);
if (pdef->subTypeString != nullptr) if (pdef->subTypeString != nullptr)
lcdDrawTextAtIndex(MODEL_SETUP_4TH_COLUMN, y, pdef->subTypeString, g_model.moduleData[EXTERNAL_MODULE].subType, menuHorizontalPosition==2 ? attr : 0); lcdDrawTextAtIndex(MODEL_SETUP_4TH_COLUMN, y, pdef->subTypeString, g_model.moduleData[moduleIdx].subType, menuHorizontalPosition==2 ? attr : 0);
} }
} }
#endif #endif
if (attr && menuHorizontalPosition == 0 && moduleIdx == EXTERNAL_MODULE) {
if (s_editMode > 0)
EXTERNAL_MODULE_OFF();
else
EXTERNAL_MODULE_ON();
}
if (attr) { if (attr) {
if (s_editMode > 0) { if (s_editMode > 0) {
switch (menuHorizontalPosition) { switch (menuHorizontalPosition) {
case 0: case 0:
{ {
uint8_t moduleType = checkIncDec(event, g_model.moduleData[EXTERNAL_MODULE].type, MODULE_TYPE_NONE, MODULE_TYPE_MAX, EE_MODEL, isExternalModuleAvailable); #if defined(HARDWARE_INTERNAL_MODULE)
IsValueAvailable checkFun = moduleIdx == 0 ? isInternalModuleAvailable : isExternalModuleAvailable;
#else
IsValueAvailable checkFun = isExternalModuleAvailable;
#endif
uint8_t moduleType = checkIncDec(event, g_model.moduleData[moduleIdx].type, MODULE_TYPE_NONE, MODULE_TYPE_MAX, EE_MODEL, checkFun);
if (checkIncDec_Ret) { if (checkIncDec_Ret) {
setModuleType(EXTERNAL_MODULE, moduleType); setModuleType(moduleIdx, moduleType);
} }
break; break;
} }
case 1: case 1:
if (isModuleDSM2(EXTERNAL_MODULE)) if (isModuleDSM2(moduleIdx))
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[EXTERNAL_MODULE].rfProtocol, DSM2_PROTO_LP45, DSM2_PROTO_DSMX); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].rfProtocol, DSM2_PROTO_LP45, DSM2_PROTO_DSMX);
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
else if (isModuleMultimodule(EXTERNAL_MODULE)) { else if (isModuleMultimodule(moduleIdx)) {
int multiRfProto = g_model.moduleData[EXTERNAL_MODULE].multi.customProto == 1 ? MODULE_SUBTYPE_MULTI_CUSTOM : g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false); int multiRfProto = g_model.moduleData[moduleIdx].multi.customProto == 1 ? MODULE_SUBTYPE_MULTI_CUSTOM : g_model.moduleData[moduleIdx].getMultiProtocol(false);
CHECK_INCDEC_MODELVAR(event, multiRfProto, MODULE_SUBTYPE_MULTI_FIRST, MODULE_SUBTYPE_MULTI_LAST); CHECK_INCDEC_MODELVAR(event, multiRfProto, MODULE_SUBTYPE_MULTI_FIRST, MODULE_SUBTYPE_MULTI_LAST);
if (checkIncDec_Ret) { if (checkIncDec_Ret) {
g_model.moduleData[EXTERNAL_MODULE].multi.customProto = (multiRfProto == MODULE_SUBTYPE_MULTI_CUSTOM); g_model.moduleData[moduleIdx].multi.customProto = (multiRfProto == MODULE_SUBTYPE_MULTI_CUSTOM);
if (!g_model.moduleData[EXTERNAL_MODULE].multi.customProto) if (!g_model.moduleData[moduleIdx].multi.customProto)
g_model.moduleData[EXTERNAL_MODULE].setMultiProtocol(multiRfProto); g_model.moduleData[moduleIdx].setMultiProtocol(multiRfProto);
g_model.moduleData[EXTERNAL_MODULE].subType = 0; g_model.moduleData[moduleIdx].subType = 0;
// Sensible default for DSM2 (same as for ppm): 7ch@22ms + Autodetect settings enabled // Sensible default for DSM2 (same as for ppm): 7ch@22ms + Autodetect settings enabled
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) { if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) {
g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode = 1; g_model.moduleData[moduleIdx].multi.autoBindMode = 1;
} }
else { else {
g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode = 0; g_model.moduleData[moduleIdx].multi.autoBindMode = 0;
} }
g_model.moduleData[EXTERNAL_MODULE].multi.optionValue = 0; g_model.moduleData[moduleIdx].multi.optionValue = 0;
} }
} }
#endif #endif
else if (isModuleR9MNonAccess(EXTERNAL_MODULE)) { else if (isModuleR9MNonAccess(moduleIdx)) {
g_model.moduleData[EXTERNAL_MODULE].subType = checkIncDec(event, g_model.moduleData[EXTERNAL_MODULE].subType, MODULE_SUBTYPE_R9M_FCC, g_model.moduleData[moduleIdx].subType = checkIncDec(event, g_model.moduleData[moduleIdx].subType, MODULE_SUBTYPE_R9M_FCC,
MODULE_SUBTYPE_R9M_LAST, EE_MODEL, isR9MModeAvailable); MODULE_SUBTYPE_R9M_LAST, EE_MODEL, isR9MModeAvailable);
} }
else { else {
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[EXTERNAL_MODULE].subType, MODULE_SUBTYPE_PXX1_ACCST_D16, MODULE_SUBTYPE_PXX1_LAST); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].subType, MODULE_SUBTYPE_PXX1_ACCST_D16, MODULE_SUBTYPE_PXX1_LAST);
} }
if (checkIncDec_Ret) { if (checkIncDec_Ret) {
g_model.moduleData[EXTERNAL_MODULE].channelsStart = 0; g_model.moduleData[moduleIdx].channelsStart = 0;
g_model.moduleData[EXTERNAL_MODULE].channelsCount = defaultModuleChannels_M8(EXTERNAL_MODULE); g_model.moduleData[moduleIdx].channelsCount = defaultModuleChannels_M8(moduleIdx);
} }
break; break;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
case 2: case 2:
if (g_model.moduleData[EXTERNAL_MODULE].multi.customProto) { if (g_model.moduleData[moduleIdx].multi.customProto) {
g_model.moduleData[EXTERNAL_MODULE].setMultiProtocol(checkIncDec(event, g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false), 0, 63, EE_MODEL)); g_model.moduleData[moduleIdx].setMultiProtocol(checkIncDec(event, g_model.moduleData[moduleIdx].getMultiProtocol(false), 0, 63, EE_MODEL));
break; break;
} else { } else {
const mm_protocol_definition *pdef = getMultiProtocolDefinition(g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false)); const mm_protocol_definition *pdef = getMultiProtocolDefinition(g_model.moduleData[moduleIdx].getMultiProtocol(false));
if (pdef->maxSubtype > 0) if (pdef->maxSubtype > 0)
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[EXTERNAL_MODULE].subType, 0, pdef->maxSubtype); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].subType, 0, pdef->maxSubtype);
} }
break; break;
case 3: case 3:
// Custom protocol, third column is subtype // Custom protocol, third column is subtype
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[EXTERNAL_MODULE].subType, 0, 7); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].subType, 0, 7);
break; break;
#endif #endif
} }
} }
#if POPUP_LEVEL > 1 #if POPUP_LEVEL > 1
else if (old_editMode > 0) { else if (old_editMode > 0) {
if (isModuleR9MNonAccess(EXTERNAL_MODULE)) { if (isModuleR9MNonAccess(moduleIdx)) {
if (g_model.moduleData[EXTERNAL_MODULE].subType > MODULE_SUBTYPE_R9M_EU) { if (g_model.moduleData[moduleIdx].subType > MODULE_SUBTYPE_R9M_EU) {
POPUP_WARNING(STR_MODULE_PROTOCOL_FLEX_WARN_LINE1); POPUP_WARNING(STR_MODULE_PROTOCOL_FLEX_WARN_LINE1);
SET_WARNING_INFO(STR_MODULE_PROTOCOL_WARN_LINE2, sizeof(TR_MODULE_PROTOCOL_WARN_LINE2) - 1, 0); SET_WARNING_INFO(STR_MODULE_PROTOCOL_WARN_LINE2, sizeof(TR_MODULE_PROTOCOL_WARN_LINE2) - 1, 0);
} }
#if POPUP_LEVEL >= 3 #if POPUP_LEVEL >= 3
else if (g_model.moduleData[EXTERNAL_MODULE].subType == MODULE_SUBTYPE_R9M_EU) { else if (g_model.moduleData[moduleIdx].subType == MODULE_SUBTYPE_R9M_EU) {
POPUP_WARNING(STR_MODULE_PROTOCOL_EU_WARN_LINE1); POPUP_WARNING(STR_MODULE_PROTOCOL_EU_WARN_LINE1);
SET_WARNING_INFO(STR_MODULE_PROTOCOL_WARN_LINE2, sizeof(TR_MODULE_PROTOCOL_WARN_LINE2) - 1, 0); SET_WARNING_INFO(STR_MODULE_PROTOCOL_WARN_LINE2, sizeof(TR_MODULE_PROTOCOL_WARN_LINE2) - 1, 0);
} }
@ -1111,11 +1155,11 @@ bool menuModelSetup(event_t event)
#endif #endif
} }
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_CHANNELS: case ITEM_MODEL_SETUP_INTERNAL_MODULE_CHANNELS:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_CHANNELS: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_CHANNELS:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
ModuleData & moduleData = g_model.moduleData[moduleIdx]; ModuleData & moduleData = g_model.moduleData[moduleIdx];
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_CHANNELRANGE); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_CHANNELRANGE);
if ((int8_t)MODULE_CHANNELS_ROWS(moduleIdx) >= 0) { if ((int8_t)MODULE_CHANNELS_ROWS(moduleIdx) >= 0) {
@ -1141,7 +1185,6 @@ bool menuModelSetup(event_t event)
} }
break; break;
} }
case ITEM_MODEL_SETUP_TRAINER_LABEL: case ITEM_MODEL_SETUP_TRAINER_LABEL:
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_TRAINER); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_TRAINER);
break; break;
@ -1149,12 +1192,15 @@ bool menuModelSetup(event_t event)
case ITEM_MODEL_SETUP_TRAINER_MODE: case ITEM_MODEL_SETUP_TRAINER_MODE:
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODE);
g_model.trainerData.mode = editChoice(MODEL_SETUP_2ND_COLUMN, y, STR_VTRAINERMODES, g_model.trainerData.mode, 0, TRAINER_MODE_MAX(), attr, event); g_model.trainerData.mode = editChoice(MODEL_SETUP_2ND_COLUMN, y, STR_VTRAINERMODES, g_model.trainerData.mode, 0, TRAINER_MODE_MAX(), attr, event);
#if defined(BLUETOOTH)
if (attr && checkIncDec_Ret) { if (attr && checkIncDec_Ret) {
bluetooth.state = BLUETOOTH_STATE_OFF; bluetooth.state = BLUETOOTH_STATE_OFF;
bluetooth.distantAddr[0] = 0; bluetooth.distantAddr[0] = 0;
} }
#endif
break; break;
#if defined(BLUETOOTH)
case ITEM_MODEL_SETUP_TRAINER_BLUETOOTH: case ITEM_MODEL_SETUP_TRAINER_BLUETOOTH:
if (g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH) { if (g_model.trainerData.mode == TRAINER_MODE_MASTER_BLUETOOTH) {
if (attr) { if (attr) {
@ -1194,6 +1240,7 @@ bool menuModelSetup(event_t event)
} }
} }
break; break;
#endif
case ITEM_MODEL_SETUP_TRAINER_CHANNELS: case ITEM_MODEL_SETUP_TRAINER_CHANNELS:
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_CHANNELRANGE); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_CHANNELRANGE);
@ -1242,10 +1289,11 @@ bool menuModelSetup(event_t event)
break; break;
#endif #endif
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_MODEL_NUM: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_MODEL_NUM:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_MODEL_NUM: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_MODEL_NUM:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_RECEIVER_NUM); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_RECEIVER_NUM);
lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, g_model.header.modelId[moduleIdx], attr | LEADING0 | LEFT, 2); lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, g_model.header.modelId[moduleIdx], attr | LEADING0 | LEFT, 2);
if (attr) { if (attr) {
@ -1261,11 +1309,11 @@ bool menuModelSetup(event_t event)
} }
} }
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_REGISTER_RANGE: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_REGISTER_RANGE:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_REGISTER_RANGE: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_REGISTER_RANGE:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODULE); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_MODULE);
drawButton(MODEL_SETUP_2ND_COLUMN, y, STR_REGISTER, (menuHorizontalPosition == 0 ? attr : 0)); drawButton(MODEL_SETUP_2ND_COLUMN, y, STR_REGISTER, (menuHorizontalPosition == 0 ? attr : 0));
drawButton(MODEL_SETUP_2ND_COLUMN + MODEL_SETUP_SET_FAILSAFE_OFS, y, STR_MODULE_RANGE, (menuHorizontalPosition == 1 ? attr : 0)); drawButton(MODEL_SETUP_2ND_COLUMN + MODEL_SETUP_SET_FAILSAFE_OFS, y, STR_MODULE_RANGE, (menuHorizontalPosition == 1 ? attr : 0));
@ -1288,8 +1336,9 @@ bool menuModelSetup(event_t event)
} }
} }
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_OPTIONS: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_OPTIONS:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_OPTIONS: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_OPTIONS:
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_OPTIONS); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, STR_OPTIONS);
drawButton(MODEL_SETUP_2ND_COLUMN, y, STR_SET, attr); drawButton(MODEL_SETUP_2ND_COLUMN, y, STR_SET, attr);
@ -1298,15 +1347,15 @@ bool menuModelSetup(event_t event)
pushMenu(menuModelModuleOptions); pushMenu(menuModelModuleOptions);
} }
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_1:
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_2: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_2:
case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_3: case ITEM_MODEL_SETUP_INTERNAL_MODULE_PXX2_RECEIVER_3:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_1: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_1:
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_2: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_2:
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_3: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_PXX2_RECEIVER_3:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
uint8_t receiverIdx = CURRENT_RECEIVER_EDITED(k); uint8_t receiverIdx = CURRENT_RECEIVER_EDITED(k);
ModuleInformation & moduleInformation = reusableBuffer.moduleSetup.pxx2.moduleInformation; ModuleInformation & moduleInformation = reusableBuffer.moduleSetup.pxx2.moduleInformation;
@ -1382,11 +1431,11 @@ bool menuModelSetup(event_t event)
} }
} }
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND: case ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_NOT_ACCESS_BIND: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_NOT_ACCESS_BIND:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
ModuleData & moduleData = g_model.moduleData[moduleIdx]; ModuleData & moduleData = g_model.moduleData[moduleIdx];
if (isModulePPM(moduleIdx)) { if (isModulePPM(moduleIdx)) {
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_PPMFRAME); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_PPMFRAME);
@ -1454,8 +1503,8 @@ bool menuModelSetup(event_t event)
drawButton(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, (moduleState[moduleIdx].mode == MODULE_MODE_RANGECHECK ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==2 ? attr : 0)); drawButton(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, (moduleState[moduleIdx].mode == MODULE_MODE_RANGECHECK ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==2 ? attr : 0));
uint8_t newFlag = 0; uint8_t newFlag = 0;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (multiBindStatus == MULTI_BIND_FINISHED) { if (getMultiBindStatus(moduleIdx) == MULTI_BIND_FINISHED) {
multiBindStatus = MULTI_NORMAL_OPERATION; setMultiBindStatus(moduleIdx, MULTI_NORMAL_OPERATION);
s_editMode = 0; s_editMode = 0;
} }
#endif #endif
@ -1488,17 +1537,17 @@ bool menuModelSetup(event_t event)
moduleState[moduleIdx].mode = newFlag; moduleState[moduleIdx].mode = newFlag;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (newFlag == MODULE_MODE_BIND) if (newFlag == MODULE_MODE_BIND)
multiBindStatus = MULTI_BIND_INITIATED; setMultiBindStatus(moduleIdx, MULTI_BIND_INITIATED);
#endif #endif
} }
} }
break; break;
} }
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_FAILSAFE: case ITEM_MODEL_SETUP_INTERNAL_MODULE_FAILSAFE:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_FAILSAFE: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_FAILSAFE:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
ModuleData & moduleData = g_model.moduleData[moduleIdx]; ModuleData & moduleData = g_model.moduleData[moduleIdx];
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_FAILSAFE); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_FAILSAFE);
lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_VFAILSAFE, moduleData.failsafeMode, menuHorizontalPosition==0 ? attr : 0); lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_VFAILSAFE, moduleData.failsafeMode, menuHorizontalPosition==0 ? attr : 0);
@ -1536,15 +1585,16 @@ bool menuModelSetup(event_t event)
} }
break; break;
} }
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_OPTIONS:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_OPTIONS: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_OPTIONS:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (isModuleMultimodule(moduleIdx)) { if (isModuleMultimodule(moduleIdx)) {
int optionValue = g_model.moduleData[moduleIdx].multi.optionValue; int optionValue = g_model.moduleData[moduleIdx].multi.optionValue;
const uint8_t multi_proto = g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true); const uint8_t multi_proto = g_model.moduleData[moduleIdx].getMultiProtocol(true);
const mm_protocol_definition *pdef = getMultiProtocolDefinition(multi_proto); const mm_protocol_definition *pdef = getMultiProtocolDefinition(multi_proto);
if (pdef->optionsstr) if (pdef->optionsstr)
lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, pdef->optionsstr); lcdDrawText(MENUS_MARGIN_LEFT + INDENT_WIDTH, y, pdef->optionsstr);
@ -1580,13 +1630,14 @@ bool menuModelSetup(event_t event)
} }
break; break;
} }
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_POWER:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_POWER: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_POWER:
{ {
uint8_t moduleIdx = CURRENT_MODULE_EDITED(k);
if (isModuleR9MNonAccess(moduleIdx)) { if (isModuleR9MNonAccess(moduleIdx)) {
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_RFPOWER); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_RFPOWER);
if(isModuleR9M_FCC_VARIANT(moduleIdx)) { if (isModuleR9M_FCC_VARIANT(moduleIdx)) {
lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_R9M_FCC_POWER_VALUES, g_model.moduleData[moduleIdx].pxx.power, LEFT | attr); lcdDrawTextAtIndex(MODEL_SETUP_2ND_COLUMN, y, STR_R9M_FCC_POWER_VALUES, g_model.moduleData[moduleIdx].pxx.power, LEFT | attr);
if (attr) if (attr)
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].pxx.power, 0, R9M_FCC_POWER_MAX); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].pxx.power, 0, R9M_FCC_POWER_MAX);
@ -1596,7 +1647,7 @@ bool menuModelSetup(event_t event)
if (attr) if (attr)
CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].pxx.power, 0, R9M_LBT_POWER_MAX); CHECK_INCDEC_MODELVAR(event, g_model.moduleData[moduleIdx].pxx.power, 0, R9M_LBT_POWER_MAX);
if (attr && s_editMode == 0 && reusableBuffer.moduleSetup.r9mPower != g_model.moduleData[moduleIdx].pxx.power) { if (attr && s_editMode == 0 && reusableBuffer.moduleSetup.r9mPower != g_model.moduleData[moduleIdx].pxx.power) {
if((reusableBuffer.moduleSetup.r9mPower + g_model.moduleData[moduleIdx].pxx.power) < 5) { //switching between mode 2 and 3 does not require rebind if ((reusableBuffer.moduleSetup.r9mPower + g_model.moduleData[moduleIdx].pxx.power) < 5) { //switching between mode 2 and 3 does not require rebind
POPUP_WARNING(STR_WARNING); POPUP_WARNING(STR_WARNING);
SET_WARNING_INFO(STR_REBIND, sizeof(TR_REBIND), 0); SET_WARNING_INFO(STR_REBIND, sizeof(TR_REBIND), 0);
} }
@ -1607,39 +1658,49 @@ bool menuModelSetup(event_t event)
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
else if (isModuleMultimodule(moduleIdx)) { else if (isModuleMultimodule(moduleIdx)) {
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_LOWPOWER); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_LOWPOWER);
g_model.moduleData[EXTERNAL_MODULE].multi.lowPowerMode = editCheckBox(g_model.moduleData[EXTERNAL_MODULE].multi.lowPowerMode, MODEL_SETUP_2ND_COLUMN, y, attr, event); g_model.moduleData[moduleIdx].multi.lowPowerMode = editCheckBox(g_model.moduleData[moduleIdx].multi.lowPowerMode, MODEL_SETUP_2ND_COLUMN, y, attr, event);
} }
#endif #endif
} }
break; break;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
#if defined(INTERNAL_MODULE_MULTI)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_AUTOBIND:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_AUTOBIND: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_AUTOBIND:
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2)
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_DSM_AUTODTECT); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_DSM_AUTODTECT);
else else
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_AUTOBIND); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MULTI_AUTOBIND);
g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode = editCheckBox(g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode, MODEL_SETUP_2ND_COLUMN, y, attr, event); g_model.moduleData[moduleIdx].multi.autoBindMode = editCheckBox(g_model.moduleData[moduleIdx].multi.autoBindMode, MODEL_SETUP_2ND_COLUMN, y, attr, event);
break; break;
#if defined(INTERNAL_MODULE_MULTI)
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_STATUS: { case ITEM_MODEL_SETUP_INTERNAL_MODULE_STATUS:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_STATUS:
{
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MODULE_STATUS); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MODULE_STATUS);
char statusText[64]; char statusText[64];
multiModuleStatus.getStatusString(statusText); getMultiModuleStatus(moduleIdx).getStatusString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
}
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SYNCSTATUS: { #if defined(INTERNAL_MODULE_MULTI)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_SYNCSTATUS:
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SYNCSTATUS:
{
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MODULE_SYNC); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_MODULE_SYNC);
char statusText[64]; char statusText[64];
multiSyncStatus.getRefreshString(statusText); getMultiSyncStatus(moduleIdx).getRefreshString(statusText);
lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText); lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
break; break;
}
} }
#endif #endif
} }
} }
@ -1656,7 +1717,7 @@ bool menuModelSetup(event_t event)
case ITEM_MODEL_SETUP_NAME: case ITEM_MODEL_SETUP_NAME:
mod_cell->setModelName(g_model.header.name); mod_cell->setModelName(g_model.header.name);
break; break;
#if defined(HARDWARE_INTERNAL_MODULE)
case ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND: case ITEM_MODEL_SETUP_INTERNAL_MODULE_NOT_ACCESS_BIND:
if (menuHorizontalPosition != 0) if (menuHorizontalPosition != 0)
break; break;
@ -1664,7 +1725,7 @@ bool menuModelSetup(event_t event)
mod_cell->setRfData(&g_model); mod_cell->setRfData(&g_model);
checkModelIdUnique(INTERNAL_MODULE); checkModelIdUnique(INTERNAL_MODULE);
break; break;
#endif
case ITEM_MODEL_SETUP_EXTERNAL_MODULE_NOT_ACCESS_BIND: case ITEM_MODEL_SETUP_EXTERNAL_MODULE_NOT_ACCESS_BIND:
if (menuHorizontalPosition != 0) if (menuHorizontalPosition != 0)
break; break;
@ -1672,6 +1733,7 @@ bool menuModelSetup(event_t event)
mod_cell->setRfData(&g_model); mod_cell->setRfData(&g_model);
if (g_model.moduleData[EXTERNAL_MODULE].type != MODULE_TYPE_NONE) if (g_model.moduleData[EXTERNAL_MODULE].type != MODULE_TYPE_NONE)
checkModelIdUnique(EXTERNAL_MODULE); checkModelIdUnique(EXTERNAL_MODULE);
break;
} }
} }
} }

View file

@ -55,10 +55,11 @@ enum MenuRadioHardwareItems {
ITEM_RADIO_HARDWARE_BATTERY_CALIB, ITEM_RADIO_HARDWARE_BATTERY_CALIB,
ITEM_RADIO_HARDWARE_RTC_BATTERY, ITEM_RADIO_HARDWARE_RTC_BATTERY,
ITEM_RADIO_HARDWARE_SERIAL_BAUDRATE, ITEM_RADIO_HARDWARE_SERIAL_BAUDRATE,
#if defined(BLUETOOTH)
ITEM_RADIO_HARDWARE_BLUETOOTH_MODE, ITEM_RADIO_HARDWARE_BLUETOOTH_MODE,
ITEM_RADIO_HARDWARE_BLUETOOTH_PAIRING_CODE, ITEM_RADIO_HARDWARE_BLUETOOTH_PAIRING_CODE,
ITEM_RADIO_HARDWARE_BLUETOOTH_NAME, ITEM_RADIO_HARDWARE_BLUETOOTH_NAME,
#endif
#if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA) #if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA)
ITEM_RADIO_HARDWARE_EXTERNAL_ANTENNA, ITEM_RADIO_HARDWARE_EXTERNAL_ANTENNA,
#endif #endif
@ -77,7 +78,11 @@ enum MenuRadioHardwareItems {
#define POTS_ROWS NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1 #define POTS_ROWS NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1
#define SWITCHES_ROWS NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1 #define SWITCHES_ROWS NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1, NAVIGATION_LINE_BY_LINE|1
#define BLUETOOTH_ROWS 0, uint8_t(g_eeGeneral.bluetoothMode != BLUETOOTH_TELEMETRY ? HIDDEN_ROW : -1), uint8_t(g_eeGeneral.bluetoothMode == BLUETOOTH_OFF ? -1 : 0) #if defined(BLUETOOTH)
#define BLUETOOTH_ROWS 0, uint8_t(g_eeGeneral.bluetoothMode != BLUETOOTH_TELEMETRY ? HIDDEN_ROW : -1), uint8_t(g_eeGeneral.bluetoothMode == BLUETOOTH_OFF ? -1 : 0),
#else
#define BLUETOOTH_ROWS
#endif
// TODO should be moved to the HAL // TODO should be moved to the HAL
#define SWITCH_TYPE_MAX(sw) ((MIXSRC_SF-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SH-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SI-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SJ-MIXSRC_FIRST_SWITCH == sw) ? SWITCH_2POS : SWITCH_3POS) #define SWITCH_TYPE_MAX(sw) ((MIXSRC_SF-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SH-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SI-MIXSRC_FIRST_SWITCH == sw || MIXSRC_SJ-MIXSRC_FIRST_SWITCH == sw) ? SWITCH_2POS : SWITCH_3POS)
@ -121,7 +126,7 @@ bool menuRadioHardware(event_t event)
0, /* max baudrate */ 0, /* max baudrate */
BLUETOOTH_ROWS, BLUETOOTH_ROWS
EXTERNAL_ANTENNA_ROW EXTERNAL_ANTENNA_ROW
@ -270,7 +275,7 @@ bool menuRadioHardware(event_t event)
} }
} }
break; break;
#if defined(BLUETOOTH)
case ITEM_RADIO_HARDWARE_BLUETOOTH_MODE: case ITEM_RADIO_HARDWARE_BLUETOOTH_MODE:
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_BLUETOOTH); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_BLUETOOTH);
g_eeGeneral.bluetoothMode = editChoice(HW_SETTINGS_COLUMN+50, y, STR_BLUETOOTH_MODES, g_eeGeneral.bluetoothMode, BLUETOOTH_OFF, BLUETOOTH_TRAINER, attr, event); g_eeGeneral.bluetoothMode = editChoice(HW_SETTINGS_COLUMN+50, y, STR_BLUETOOTH_MODES, g_eeGeneral.bluetoothMode, BLUETOOTH_OFF, BLUETOOTH_TRAINER, attr, event);
@ -285,6 +290,7 @@ bool menuRadioHardware(event_t event)
lcdDrawText(MENUS_MARGIN_LEFT, y, STR_NAME); lcdDrawText(MENUS_MARGIN_LEFT, y, STR_NAME);
editName(HW_SETTINGS_COLUMN+50, y, g_eeGeneral.bluetoothName, LEN_BLUETOOTH_NAME, event, attr); editName(HW_SETTINGS_COLUMN+50, y, g_eeGeneral.bluetoothName, LEN_BLUETOOTH_NAME, event, attr);
break; break;
#endif
#if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA) #if defined(INTERNAL_MODULE_PXX1) && defined(EXTERNAL_ANTENNA)
case ITEM_RADIO_HARDWARE_EXTERNAL_ANTENNA: case ITEM_RADIO_HARDWARE_EXTERNAL_ANTENNA:

View file

@ -19,7 +19,8 @@
*/ */
#include <stdio.h> #include <stdio.h>
#include <io/frsky_firmware_update.h> #include "io/frsky_firmware_update.h"
#include "io/multi_firmware_update.h"
#include "opentx.h" #include "opentx.h"
#include "storage/modelslist.h" #include "storage/modelslist.h"
@ -173,6 +174,18 @@ void onSdManagerMenu(const char * result)
FrskyDeviceFirmwareUpdate device(SPORT_MODULE); FrskyDeviceFirmwareUpdate device(SPORT_MODULE);
device.flashFirmware(lfn); device.flashFirmware(lfn);
} }
#if defined(MULTIMODULE)
#if defined(INTERNAL_MODULE_MULTI)
else if (result == STR_FLASH_INTERNAL_MULTI) {
getSelectionFullPath(lfn);
multiFlashFirmware(INTERNAL_MODULE, lfn);
}
#endif
else if (result == STR_FLASH_EXTERNAL_MULTI) {
getSelectionFullPath(lfn);
multiFlashFirmware(EXTERNAL_MODULE, lfn);
}
#endif
#if defined(LUA) #if defined(LUA)
else if (result == STR_EXECUTE_FILE) { else if (result == STR_EXECUTE_FILE) {
getSelectionFullPath(lfn); getSelectionFullPath(lfn);
@ -258,9 +271,22 @@ bool menuRadioSdManager(event_t _event)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE);
POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MODULE);
} }
else if (!READ_ONLY() && !strcasecmp(ext, UPDATE_FIRMWARE_EXT)) { #if defined(MULTIMODULE)
else if (!READ_ONLY() && !strcasecmp(ext, MULTI_FIRMWARE_EXT)) {
TCHAR lfn[_MAX_LFN + 1];
getSelectionFullPath(lfn);
MultiFirmwareInformation information;
if (information.readMultiFirmwareInformation(line) == nullptr) {
#if defined(INTERNAL_MODULE_MULTI)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MULTI);
#endif
POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MULTI);
}
#endif
}
else if (!READ_ONLY() && !strcasecmp(ext, FRSKY_FIRMWARE_EXT)) {
FrSkyFirmwareInformation information; FrSkyFirmwareInformation information;
if (readFirmwareInformation(line, information) == nullptr) { if (readFrSkyFirmwareInformation(line, information) == nullptr) {
if (information.productFamily == FIRMWARE_FAMILY_INTERNAL_MODULE) if (information.productFamily == FIRMWARE_FAMILY_INTERNAL_MODULE)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE);
if (information.productFamily == FIRMWARE_FAMILY_EXTERNAL_MODULE) if (information.productFamily == FIRMWARE_FAMILY_EXTERNAL_MODULE)

View file

@ -176,7 +176,9 @@ class DefaultTheme: public Theme
calibTrackpBackground = BitmapBuffer::load(getThemePath("trackp_background.png")); calibTrackpBackground = BitmapBuffer::load(getThemePath("trackp_background.png"));
delete calibHorus; delete calibHorus;
#if defined(PCBX10) #if defined(RADIO_T16)
calibHorus = BitmapBuffer::load(getThemePath("t16.bmp"));
#elif defined(PCBX10)
if(STICKS_PWM_ENABLED()) { if(STICKS_PWM_ENABLED()) {
calibHorus = BitmapBuffer::load(getThemePath("X10S.bmp")); calibHorus = BitmapBuffer::load(getThemePath("X10S.bmp"));
} }

View file

@ -20,6 +20,7 @@
#include "opentx.h" #include "opentx.h"
#include "io/frsky_firmware_update.h" #include "io/frsky_firmware_update.h"
#include "io/multi_firmware_update.h"
#define NODE_TYPE(fname) fname[SD_SCREEN_FILE_LENGTH+1] #define NODE_TYPE(fname) fname[SD_SCREEN_FILE_LENGTH+1]
#define IS_DIRECTORY(fname) ((bool)(!NODE_TYPE(fname))) #define IS_DIRECTORY(fname) ((bool)(!NODE_TYPE(fname)))
@ -219,6 +220,16 @@ void onSdManagerMenu(const char * result)
FrskyDeviceFirmwareUpdate device(SPORT_MODULE); FrskyDeviceFirmwareUpdate device(SPORT_MODULE);
device.flashFirmware(lfn); device.flashFirmware(lfn);
} }
#if defined(INTERNAL_MODULE_MULTI)
else if (result == STR_FLASH_INTERNAL_MULTI) {
getSelectionFullPath(lfn);
multiFlashFirmware(INTERNAL_MODULE, lfn);
}
#endif
else if (result == STR_FLASH_EXTERNAL_MULTI) {
getSelectionFullPath(lfn);
multiFlashFirmware(EXTERNAL_MODULE, lfn);
}
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
else if (result == STR_FLASH_BLUETOOTH_MODULE) { else if (result == STR_FLASH_BLUETOOTH_MODULE) {
getSelectionFullPath(lfn); getSelectionFullPath(lfn);
@ -340,6 +351,9 @@ void menuRadioSdManager(event_t _event)
break; break;
} }
#endif #endif
TCHAR lfn[_MAX_LFN + 1];
getSelectionFullPath(lfn);
if (SD_CARD_PRESENT() && s_editMode <= 0) { if (SD_CARD_PRESENT() && s_editMode <= 0) {
killEvents(_event); killEvents(_event);
int index = menuVerticalPosition - HEADER_LINE - menuVerticalOffset; int index = menuVerticalPosition - HEADER_LINE - menuVerticalOffset;
@ -364,10 +378,19 @@ void menuRadioSdManager(event_t _event)
POPUP_MENU_ADD_ITEM(STR_EXECUTE_FILE); POPUP_MENU_ADD_ITEM(STR_EXECUTE_FILE);
} }
#endif #endif
#if defined(MULTIMODULE) && !defined(DISABLE_MULTI_UPDATE)
if (!READ_ONLY() && !strcasecmp(ext, MULTI_FIRMWARE_EXT)) {
MultiFirmwareInformation information;
if (information.readMultiFirmwareInformation(line) == nullptr) {
#if defined(INTERNAL_MODULE_MULTI)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MULTI);
#endif
POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MULTI);
}
}
#endif
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
else if (!READ_ONLY() && !strcasecmp(ext, FIRMWARE_EXT)) { if (!READ_ONLY() && !strcasecmp(ext, FIRMWARE_EXT)) {
TCHAR lfn[_MAX_LFN + 1];
getSelectionFullPath(lfn);
if (isBootloader(lfn)) { if (isBootloader(lfn)) {
POPUP_MENU_ADD_ITEM(STR_FLASH_BOOTLOADER); POPUP_MENU_ADD_ITEM(STR_FLASH_BOOTLOADER);
} }
@ -378,9 +401,9 @@ void menuRadioSdManager(event_t _event)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE);
POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_EXTERNAL_MODULE);
} }
else if (!READ_ONLY() && !strcasecmp(ext, UPDATE_FIRMWARE_EXT)) { else if (!READ_ONLY() && !strcasecmp(ext, FRSKY_FIRMWARE_EXT)) {
FrSkyFirmwareInformation information; FrSkyFirmwareInformation information;
if (readFirmwareInformation(line, information) == nullptr) { if (readFrSkyFirmwareInformation(line, information) == nullptr) {
if (information.productFamily == FIRMWARE_FAMILY_INTERNAL_MODULE) if (information.productFamily == FIRMWARE_FAMILY_INTERNAL_MODULE)
POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE); POPUP_MENU_ADD_ITEM(STR_FLASH_INTERNAL_MODULE);
if (information.productFamily == FIRMWARE_FAMILY_EXTERNAL_MODULE) if (information.productFamily == FIRMWARE_FAMILY_EXTERNAL_MODULE)

View file

@ -567,6 +567,13 @@ bool isInternalModuleAvailable(int moduleType)
if (moduleType == MODULE_TYPE_NONE) if (moduleType == MODULE_TYPE_NONE)
return true; return true;
#if defined(INTERNAL_MODULE_MULTI)
if (moduleType == MODULE_TYPE_MULTIMODULE)
return true;
else
return false;
#endif
if (moduleType == MODULE_TYPE_XJT_PXX1) { if (moduleType == MODULE_TYPE_XJT_PXX1) {
#if defined(PXX1) && defined(INTERNAL_MODULE_PXX1) #if defined(PXX1) && defined(INTERNAL_MODULE_PXX1)
return !isModuleUsingSport(EXTERNAL_MODULE, g_model.moduleData[EXTERNAL_MODULE].type); return !isModuleUsingSport(EXTERNAL_MODULE, g_model.moduleData[EXTERNAL_MODULE].type);
@ -584,7 +591,6 @@ bool isInternalModuleAvailable(int moduleType)
return true; return true;
#endif #endif
} }
return false; return false;
} }
#endif #endif

View file

@ -116,10 +116,33 @@ void drawFatalErrorScreen(const char * message);
void runFatalErrorScreen(const char * message); void runFatalErrorScreen(const char * message);
#endif #endif
#define IF_INTERNAL_MODULE_ON(x) (IS_INTERNAL_MODULE_ENABLED() ? (uint8_t)(x) : HIDDEN_ROW)
#define IF_MODULE_ON(moduleIndex, x) (IS_MODULE_ENABLED(moduleIndex) ? (uint8_t)(x) : HIDDEN_ROW)
// model_setup Defines that are used in all uis in the same way // model_setup Defines that are used in all uis in the same way
#define INTERNAL_MODULE_CHANNELS_ROWS IF_INTERNAL_MODULE_ON((uint8_t)1)
#define EXTERNAL_MODULE_CHANNELS_ROWS IF_EXTERNAL_MODULE_ON((isModuleDSM2(EXTERNAL_MODULE) || isModuleCrossfire(EXTERNAL_MODULE) || isModuleSBUS(EXTERNAL_MODULE) || (isModuleMultimodule(EXTERNAL_MODULE) && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) != MODULE_SUBTYPE_MULTI_DSM2)) ? (uint8_t)0 : (uint8_t)1) inline uint8_t MODULE_BIND_ROWS(int moduleIdx)
#define EXTERNAL_MODULE_BIND_ROWS (isModuleXJTD8(EXTERNAL_MODULE) || isModuleSBUS(EXTERNAL_MODULE)) ? (uint8_t)1 : (isModulePPM(EXTERNAL_MODULE) || isModulePXX1(EXTERNAL_MODULE) || isModulePXX2(EXTERNAL_MODULE) || isModuleDSM2(EXTERNAL_MODULE) || isModuleMultimodule(EXTERNAL_MODULE)) ? (uint8_t)2 : HIDDEN_ROW {
if (isModuleXJTD8(moduleIdx) || isModuleSBUS(moduleIdx))
return 1;
if (isModulePPM(moduleIdx) || isModulePXX1(moduleIdx) || isModulePXX2(moduleIdx) || isModuleDSM2(moduleIdx) || isModuleMultimodule(moduleIdx))
return 2;
else
return HIDDEN_ROW;
}
inline uint8_t MODULE_CHANNELS_ROWS(int moduleIdx)
{
if (!IS_MODULE_ENABLED(moduleIdx))
return HIDDEN_ROW;
if (isModuleDSM2(moduleIdx) || isModuleCrossfire(moduleIdx) || isModuleSBUS(moduleIdx) || (isModuleMultimodule(moduleIdx) && g_model.moduleData[moduleIdx].getMultiProtocol(true) != MODULE_SUBTYPE_MULTI_DSM2))
return 0;
else
return 1;
}
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
// When using packed, the pointer in here end up not being aligned, which clang and gcc complain about // When using packed, the pointer in here end up not being aligned, which clang and gcc complain about
@ -132,9 +155,9 @@ struct mm_protocol_definition {
const char *optionsstr; const char *optionsstr;
}; };
const mm_protocol_definition *getMultiProtocolDefinition (uint8_t protocol); const mm_protocol_definition *getMultiProtocolDefinition (uint8_t protocol);
#define MULTIMODULE_STATUS_ROWS isModuleMultimodule(EXTERNAL_MODULE) ? TITLE_ROW : HIDDEN_ROW, (isModuleMultimodule(EXTERNAL_MODULE) && multiSyncStatus.isValid()) ? TITLE_ROW : HIDDEN_ROW, #define MULTIMODULE_STATUS_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? TITLE_ROW : HIDDEN_ROW, (isModuleMultimodule(moduleIdx) && getMultiSyncStatus(moduleIdx).isValid()) ? TITLE_ROW : HIDDEN_ROW,
#define MULTIMODULE_MODULE_ROWS isModuleMultimodule(EXTERNAL_MODULE) ? (uint8_t) 0 : HIDDEN_ROW, #define MULTIMODULE_MODULE_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? (uint8_t) 0 : HIDDEN_ROW,
#define MULTIMODULE_MODE_ROWS(x) (g_model.moduleData[x].multi.customProto) ? (uint8_t) 3 :MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[x].getMultiProtocol(true)) ? (uint8_t)2 : (uint8_t)1 #define MULTIMODULE_MODE_ROWS(moduleIdx) (g_model.moduleData[moduleIdx].multi.customProto) ? (uint8_t) 3 :MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[moduleIdx].getMultiProtocol(true)) ? (uint8_t)2 : (uint8_t)1
inline bool MULTIMODULE_HAS_SUBTYPE(uint8_t moduleIdx) inline bool MULTIMODULE_HAS_SUBTYPE(uint8_t moduleIdx)
{ {
return getMultiProtocolDefinition(moduleIdx)->maxSubtype > 0; return getMultiProtocolDefinition(moduleIdx)->maxSubtype > 0;
@ -147,21 +170,21 @@ inline uint8_t MULTIMODULE_RFPROTO_COLUMNS(uint8_t moduleIdx)
return (g_model.moduleData[moduleIdx].multi.customProto ? (uint8_t) 2 : MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[moduleIdx].getMultiProtocol(true)) ? (uint8_t) 1 : 0); return (g_model.moduleData[moduleIdx].multi.customProto ? (uint8_t) 2 : MULTIMODULE_HAS_SUBTYPE(g_model.moduleData[moduleIdx].getMultiProtocol(true)) ? (uint8_t) 1 : 0);
#endif #endif
} }
#define MULTIMODULE_SUBTYPE_ROWS(x) isModuleMultimodule(x) ? MULTIMODULE_RFPROTO_COLUMNS(x) : HIDDEN_ROW, #define MULTIMODULE_SUBTYPE_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? MULTIMODULE_RFPROTO_COLUMNS(moduleIdx) : HIDDEN_ROW,
#define MULTIMODULE_HASOPTIONS(x) (getMultiProtocolDefinition(x)->optionsstr != nullptr) #define MULTIMODULE_HASOPTIONS(moduleIdx) (getMultiProtocolDefinition(moduleIdx)->optionsstr != nullptr)
#define MULTIMODULE_OPTIONS_ROW (isModuleMultimodule(EXTERNAL_MODULE) && MULTIMODULE_HASOPTIONS(g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true))) ? (uint8_t) 0: HIDDEN_ROW #define MULTIMODULE_OPTIONS_ROW(moduleIdx) (isModuleMultimodule(moduleIdx) && MULTIMODULE_HASOPTIONS(g_model.moduleData[moduleIdx].getMultiProtocol(true))) ? (uint8_t) 0: HIDDEN_ROW
#else #else
#define MULTIMODULE_STATUS_ROWS #define MULTIMODULE_STATUS_ROWS(moduleIdx)
#define MULTIMODULE_MODULE_ROWS #define MULTIMODULE_MODULE_ROWS(moduleIdx)
#define MULTIMODULE_SUBTYPE_ROWS(x) #define MULTIMODULE_SUBTYPE_ROWS(moduleIdx)
#define MULTIMODULE_MODE_ROWS(x) (uint8_t)0 #define MULTIMODULE_MODE_ROWS(moduleIdx) (uint8_t)0
#define MULTIMODULE_OPTIONS_ROW HIDDEN_ROW #define MULTIMODULE_OPTIONS_ROW(moduleIdx) HIDDEN_ROW
#endif #endif
#define FAILSAFE_ROWS(x) isModuleFailsafeAvailable(x) ? (g_model.moduleData[x].failsafeMode==FAILSAFE_CUSTOM ? (uint8_t)1 : (uint8_t)0) : HIDDEN_ROW #define FAILSAFE_ROWS(moduleIdx) isModuleFailsafeAvailable(moduleIdx) ? (g_model.moduleData[moduleIdx].failsafeMode==FAILSAFE_CUSTOM ? (uint8_t)1 : (uint8_t)0) : HIDDEN_ROW
#define EXTERNAL_MODULE_OPTION_ROW (isModuleR9MNonAccess(EXTERNAL_MODULE) || isModuleSBUS(EXTERNAL_MODULE) ? TITLE_ROW : MULTIMODULE_OPTIONS_ROW) #define MODULE_OPTION_ROW(moduleIdx) (isModuleR9MNonAccess(moduleIdx) || isModuleSBUS(moduleIdx) ? TITLE_ROW : MULTIMODULE_OPTIONS_ROW(moduleIdx))
#define EXTERNAL_MODULE_POWER_ROW (isModuleMultimodule(EXTERNAL_MODULE) || isModuleR9MNonAccess(EXTERNAL_MODULE)) ? (isModuleR9MLiteNonPro(EXTERNAL_MODULE) ? (isModuleR9M_FCC_VARIANT(EXTERNAL_MODULE) ? READONLY_ROW : (uint8_t)0) : (uint8_t)0) : HIDDEN_ROW #define MODULE_POWER_ROW(moduleIdx) (isModuleMultimodule(moduleIdx) || isModuleR9MNonAccess(moduleIdx)) ? (isModuleR9MLiteNonPro(moduleIdx) ? (isModuleR9M_FCC_VARIANT(moduleIdx) ? READONLY_ROW : (uint8_t)0) : (uint8_t)0) : HIDDEN_ROW
void editStickHardwareSettings(coord_t x, coord_t y, int idx, event_t event, LcdFlags flags); void editStickHardwareSettings(coord_t x, coord_t y, int idx, event_t event, LcdFlags flags);

View file

@ -33,7 +33,7 @@
#define PRIM_END_DOWNLOAD 0x83 #define PRIM_END_DOWNLOAD 0x83
#define PRIM_DATA_CRC_ERR 0x84 #define PRIM_DATA_CRC_ERR 0x84
const char * readFirmwareInformation(const char * filename, FrSkyFirmwareInformation & data) const char * readFrSkyFirmwareInformation(const char * filename, FrSkyFirmwareInformation & data)
{ {
FIL file; FIL file;
UINT count; UINT count;
@ -305,7 +305,7 @@ const char * FrskyDeviceFirmwareUpdate::doFlashFirmware(const char * filename)
} }
const char * ext = getFileExtension(filename); const char * ext = getFileExtension(filename);
if (ext && !strcasecmp(ext, UPDATE_FIRMWARE_EXT)) { if (ext && !strcasecmp(ext, FRSKY_FIRMWARE_EXT)) {
if (f_read(&file, &information, sizeof(FrSkyFirmwareInformation), &count) != FR_OK || count != sizeof(FrSkyFirmwareInformation)) { if (f_read(&file, &information, sizeof(FrSkyFirmwareInformation), &count) != FR_OK || count != sizeof(FrSkyFirmwareInformation)) {
f_close(&file); f_close(&file);
return "Format error"; return "Format error";
@ -321,7 +321,7 @@ const char * FrskyDeviceFirmwareUpdate::doFlashFirmware(const char * filename)
if (module == INTERNAL_MODULE && information.productId == FIRMWARE_ID_XJT) { if (module == INTERNAL_MODULE && information.productId == FIRMWARE_ID_XJT) {
INTERNAL_MODULE_ON(); INTERNAL_MODULE_ON();
RTOS_WAIT_MS(1); RTOS_WAIT_MS(1);
intmoduleSerialStart(38400, true); intmoduleSerialStart(38400, true, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b);
GPIO_SetBits(INTMODULE_BOOTCMD_GPIO, INTMODULE_BOOTCMD_GPIO_PIN); GPIO_SetBits(INTMODULE_BOOTCMD_GPIO, INTMODULE_BOOTCMD_GPIO_PIN);
result = uploadFileToHorusXJT(filename, &file); result = uploadFileToHorusXJT(filename, &file);
GPIO_ResetBits(INTMODULE_BOOTCMD_GPIO, INTMODULE_BOOTCMD_GPIO_PIN); GPIO_ResetBits(INTMODULE_BOOTCMD_GPIO, INTMODULE_BOOTCMD_GPIO_PIN);
@ -336,7 +336,7 @@ const char * FrskyDeviceFirmwareUpdate::doFlashFirmware(const char * filename)
// this ifdef can be removed if we use .frsk instead of .frk // this ifdef can be removed if we use .frsk instead of .frk
// theorically it should be possible to use an ISRM module in an XLite // theorically it should be possible to use an ISRM module in an XLite
case INTERNAL_MODULE: case INTERNAL_MODULE:
intmoduleSerialStart(57600, true); intmoduleSerialStart(57600, true, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b);
break; break;
#endif #endif

View file

@ -53,7 +53,7 @@ PACK(struct FrSkyFirmwareInformation {
uint16_t crc; uint16_t crc;
}); });
const char * readFirmwareInformation(const char * filename, FrSkyFirmwareInformation & data); const char * readFrSkyFirmwareInformation(const char * filename, FrSkyFirmwareInformation & data);
class FrskyDeviceFirmwareUpdate { class FrskyDeviceFirmwareUpdate {
enum State { enum State {

View file

@ -0,0 +1,447 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* 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.
*/
#if !defined(DISABLE_MULTI_UPDATE)
#include "opentx.h"
#include "multi_firmware_update.h"
#include "stk500.h"
#include "debug.h"
#define UPDATE_MULTI_EXT_BIN ".bin"
//#define DEBUG_EXT_MODULE_FLASH
class MultiFirmwareUpdateDriver
{
public:
MultiFirmwareUpdateDriver() = default;
const char* flashFirmware(FIL* file, const char* label) const;
protected:
virtual void init() const = 0;
virtual bool getByte(uint8_t& byte) const = 0;
virtual void sendByte(uint8_t byte) const = 0;
virtual void clear() const = 0;
private:
bool getRxByte(uint8_t& byte) const;
bool checkRxByte(uint8_t byte) const;
const char * waitForInitialSync() const;
const char * getDeviceSignature(uint8_t* signature) const;
const char * loadAddress(uint32_t offset) const;
const char * progPage(uint8_t* buffer, uint16_t size) const;
};
#if defined(INTERNAL_MODULE_MULTI)
class MultiInternalUpdateDriver: public MultiFirmwareUpdateDriver
{
public:
MultiInternalUpdateDriver() = default;
protected:
void init() const override
{
INTERNAL_MODULE_ON();
intmoduleSerialStart(57600, true, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b);
}
bool getByte(uint8_t& byte) const override
{
return intmoduleFifo.pop(byte);
}
void sendByte(uint8_t byte) const override
{
intmoduleSendByte(byte);
}
void clear() const override
{
intmoduleFifo.clear();
}
};
static const MultiInternalUpdateDriver multiInternalUpdateDriver;
#endif
class MultiExternalUpdateDriver: public MultiFirmwareUpdateDriver
{
public:
MultiExternalUpdateDriver() = default;
protected:
void init() const override
{
#if !defined(EXTMODULE_USART)
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = EXTMODULE_TX_GPIO_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(EXTMODULE_TX_GPIO, &GPIO_InitStructure);
#endif
EXTERNAL_MODULE_ON();
telemetryPortInit(57600, TELEMETRY_SERIAL_DEFAULT);
}
bool getByte(uint8_t& byte) const override
{
return telemetryGetByte(&byte);
}
void sendByte(uint8_t byte) const override
{
extmoduleSendInvertedByte(byte);
}
void clear() const override
{
telemetryClearFifo();
}
};
static const MultiExternalUpdateDriver multiExternalUpdateDriver;
bool MultiFirmwareUpdateDriver::getRxByte(uint8_t& byte) const
{
uint16_t time;
time = getTmr2MHz() ;
while ( (uint16_t) (getTmr2MHz() - time) < 25000 ) { // 12.5mS
if (getByte(byte)) {
#if defined(DEBUG_EXT_MODULE_FLASH)
TRACE("[RX] 0x%X", byte);
#endif
return true;
}
}
byte = 0;
return false;
}
bool MultiFirmwareUpdateDriver::checkRxByte(uint8_t byte) const
{
uint8_t rxchar;
return getRxByte(rxchar) ? rxchar == byte : false;
}
const char * MultiFirmwareUpdateDriver::waitForInitialSync() const
{
clear();
uint8_t byte;
int retries = 1000;
do {
// Send sync request
sendByte(STK_GET_SYNC);
sendByte(CRC_EOP);
getRxByte(byte);
wdt_reset();
} while((byte != STK_INSYNC) && --retries);
if (!retries) {
return "NoSync";
}
if (byte != STK_INSYNC) {
return "NoSync";
}
if (!checkRxByte(STK_OK)) {
return "NoSync";
}
return nullptr;
}
const char * MultiFirmwareUpdateDriver::getDeviceSignature(uint8_t* signature) const
{
// Read signature
sendByte(STK_READ_SIGN);
sendByte(CRC_EOP);
if (!checkRxByte(STK_INSYNC))
return "NoSync";
for (uint8_t i=0; i<4; i++) {
if (!getRxByte(signature[i])) {
return "NoSignature";
}
}
return nullptr;
}
const char * MultiFirmwareUpdateDriver::loadAddress(uint32_t offset) const
{
sendByte(STK_LOAD_ADDRESS);
sendByte(offset & 0xFF); // low byte
sendByte(offset >> 8); // high byte
sendByte(CRC_EOP);
if (!checkRxByte(STK_INSYNC) || !checkRxByte(STK_OK)) {
return "NoSync";
}
return nullptr;
}
const char * MultiFirmwareUpdateDriver::progPage(uint8_t* buffer, uint16_t size) const
{
sendByte(STK_PROG_PAGE);
// page size 256
sendByte(1);
sendByte(0);
// flash/eeprom flag
sendByte(0);
// #if defined(DEBUG_EXT_MODULE_FLASH)
// TRACE("writing at 0x%X", writeOffset << 1);
// #endif
for (uint16_t i=0; i < size; i++) {
sendByte(buffer[i]);
}
sendByte(CRC_EOP);
if (!checkRxByte(STK_INSYNC))
return "NoSync";
uint8_t byte;
uint8_t retries = 4;
do {
getRxByte(byte);
wdt_reset();
} while(!byte && --retries);
if (!retries || (byte != STK_OK))
return "NoPageSync";
return nullptr;
}
const char * MultiFirmwareUpdateDriver::flashFirmware(FIL* file, const char* label) const
{
const char* result = nullptr;
init();
/* wait 500ms for power on */
watchdogSuspend(500);
RTOS_WAIT_MS(500);
result = waitForInitialSync();
if (result != nullptr)
return result;
unsigned char signature[4]; // 3 bytes signature + STK_OK
result = getDeviceSignature(signature);
if (result != nullptr)
return result;
uint8_t buffer[256]; // page size = 256
uint32_t writeOffset = 0x1000; // start offset (word address)
while (!f_eof(file)) {
drawProgressScreen(label, STR_WRITING, file->fptr, file->obj.objsize);
UINT count=0;
memclear(buffer, 256);
if (f_read(file, buffer, 256, &count) != FR_OK) {
result = "Error reading file";
break;
}
if (!count)
break;
clear();
result = loadAddress(writeOffset);
if (result != nullptr) {
break;
}
result = progPage(buffer, 256);
if (result != nullptr) {
break;
}
writeOffset += 128; // page / 2
}
if (f_eof(file)) {
drawProgressScreen(label, STR_WRITING, file->fptr, file->obj.objsize);
}
sendByte(STK_LEAVE_PROGMODE);
sendByte(CRC_EOP);
// eat last sync byte
checkRxByte(STK_INSYNC);
return result;
}
// example : multi-stm-bcsid-01020176
#define MULTI_SIGN_SIZE 24
#define MULTI_SIGN_BOOTLOADER_SUPPORT_OFFSET 10
#define MULTI_SIGN_BOOTLOADER_CHECK_OFFSET 11
#define MULTI_SIGN_TELEM_TYPE_OFFSET 12
#define MULTI_SIGN_TELEM_INVERSION_OFFSET 13
#define MULTI_SIGN_VERSION_OFFSET 15
const char * MultiFirmwareInformation::readMultiFirmwareInformation(const char * filename)
{
FIL file;
f_open(&file, filename, FA_READ);
char buffer[MULTI_SIGN_SIZE];
UINT count;
f_lseek(&file, f_size(&file) - MULTI_SIGN_SIZE);
if (f_read(&file, buffer, MULTI_SIGN_SIZE, &count) != FR_OK || count != MULTI_SIGN_SIZE) {
return "Error reading file";
}
if(!memcmp(buffer, "multi-stm", 9))
boardType = FIRMWARE_MULTI_STM;
else if (!memcmp(buffer, "multi-avr", 9))
boardType = FIRMWARE_MULTI_AVR;
else if (!memcmp(buffer, "multi-orx", 9))
boardType = FIRMWARE_MULTI_ORX;
else
return "Wrong format";
if(buffer[MULTI_SIGN_BOOTLOADER_SUPPORT_OFFSET] == 'b')
optibootSupport = true;
else
optibootSupport = false;
if(buffer[MULTI_SIGN_BOOTLOADER_CHECK_OFFSET] == 'c')
bootloaderCheck = true;
else
bootloaderCheck = false;
if(buffer[MULTI_SIGN_TELEM_TYPE_OFFSET] == 't')
telemetryType = FIRMWARE_MULTI_TELEM_MULTI;
else if (buffer[MULTI_SIGN_TELEM_TYPE_OFFSET] == 's')
telemetryType = FIRMWARE_MULTI_TELEM_STATUS;
else
telemetryType = FIRMWARE_MULTI_TELEM_NONE;
if(buffer[MULTI_SIGN_TELEM_INVERSION_OFFSET] == 'i')
telemetryInversion = true;
else
telemetryInversion = false;
return nullptr;
}
const char * multiFlashFirmware(uint8_t moduleIdx, const char * filename)
{
FIL file;
const char * result = nullptr;
const char * ext = getFileExtension(filename);
if (ext && strcasecmp(ext, UPDATE_MULTI_EXT_BIN)) {
return "Wrong file extension";
}
if (f_open(&file, filename, FA_READ) != FR_OK) {
return "Error opening file";
}
pausePulses();
#if defined(INTERNAL_MODULE_MULTI)
uint8_t intPwr = IS_INTERNAL_MODULE_ON();
INTERNAL_MODULE_OFF();
#endif
uint8_t extPwr = IS_EXTERNAL_MODULE_ON();
EXTERNAL_MODULE_OFF();
SPORT_UPDATE_POWER_OFF();
drawProgressScreen(getBasename(filename), STR_DEVICE_RESET, 0, 0);
/* wait 2s off */
watchdogSuspend(2000);
RTOS_WAIT_MS(2000);
// TODO: real update here
const MultiFirmwareUpdateDriver* driver = &multiExternalUpdateDriver;
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
driver = &multiInternalUpdateDriver;
#endif
result = driver->flashFirmware(&file, getBasename(filename));
f_close(&file);
AUDIO_PLAY(AU_SPECIAL_SOUND_BEEP1 );
BACKLIGHT_ENABLE();
if (result) {
POPUP_WARNING(STR_FIRMWARE_UPDATE_ERROR);
SET_WARNING_INFO(result, strlen(result), 0);
}
else {
POPUP_INFORMATION(STR_FIRMWARE_UPDATE_SUCCESS);
}
INTERNAL_MODULE_OFF();
EXTERNAL_MODULE_OFF();
SPORT_UPDATE_POWER_OFF();
/* wait 2s off */
watchdogSuspend(2000);
RTOS_WAIT_MS(2000);
telemetryClearFifo();
#if defined(INTERNAL_MODULE_MULTI)
if (intPwr) {
INTERNAL_MODULE_ON();
setupPulsesInternalModule();
}
#endif
if (extPwr) {
EXTERNAL_MODULE_ON();
setupPulsesExternalModule();
}
//state = SPORT_IDLE;
resumePulses();
return result;
}
#endif

View file

@ -0,0 +1,96 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* 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.
*/
#ifndef OPENTX_MULTI_FIRMWARE_H
#define OPENTX_MULTI_FIRMWARE_H
/* Signature format is multi-[board type]-[bootloader support][check for bootloader][multi telemetry type][telemetry inversion][debug]-[firmware version]
Where:
[board type] is avr, stm, or orx
[bootloader support] b for optiboot (AVR) / USB (STM) or u (unsupported)
[check for bootloader] is c (CHECK_FOR_BOOTLOADER) or u (undefined)
[telemetry type] is t (MULTI_TELEMETRY), s (MULTI_STATUS), or u (undefined) for neither
[telemetry inversion] is i (INVERT_TELEMETRY) or u (undefined)
[firmware version] is the version padded to two bytes per segment, without seperators e.g. 01020176
For example: REM multi-stm-bcsid-01020176
*/
class MultiFirmwareInformation {
public:
enum MultiFirmwareBoardType {
FIRMWARE_MULTI_STM,
FIRMWARE_MULTI_AVR,
FIRMWARE_MULTI_ORX,
};
enum MultiFirmwareTelemetryType {
FIRMWARE_MULTI_TELEM_MULTI,
FIRMWARE_MULTI_TELEM_STATUS,
FIRMWARE_MULTI_TELEM_NONE,
};
bool isMultiStmFirmware() const
{
return boardType == FIRMWARE_MULTI_STM;
}
bool isMultiAvrFirmware() const
{
return boardType == FIRMWARE_MULTI_AVR;
}
bool isMultiOrxFirmware() const
{
return boardType == FIRMWARE_MULTI_ORX;
}
bool isMultiWithBootloaderFirmware() const
{
return optibootSupport;
}
bool isMultiInternalFirmware() const
{
return (boardType == FIRMWARE_MULTI_STM && telemetryInversion == false && optibootSupport == true && telemetryType == FIRMWARE_MULTI_TELEM_STATUS);
}
bool isMultiExternalFirmware() const
{
return (telemetryInversion == false && optibootSupport == true && telemetryType == FIRMWARE_MULTI_TELEM_STATUS);
}
const char * readMultiFirmwareInformation(const char * filename);
private:
uint8_t boardType;
uint8_t optibootSupport;
uint8_t bootloaderCheck;
uint8_t telemetryType;
uint8_t telemetryInversion;
uint8_t firmwareVersionMajor;
uint8_t firmwareVersionMinor;
uint8_t firmwareVersionRevision;
uint8_t firmwareVersionSubRevision;
};
const char* multiFlashFirmware(uint8_t module, const char * filename);
#endif //OPENTX_MULTI_FIRMWARE_H

39
radio/src/io/stk500.h Normal file
View file

@ -0,0 +1,39 @@
/* STK500 constants list, from AVRDUDE */
#define STK_OK 0x10
#define STK_FAILED 0x11 // Not used
#define STK_UNKNOWN 0x12 // Not used
#define STK_NODEVICE 0x13 // Not used
#define STK_INSYNC 0x14 // ' '
#define STK_NOSYNC 0x15 // Not used
#define ADC_CHANNEL_ERROR 0x16 // Not used
#define ADC_MEASURE_OK 0x17 // Not used
#define PWM_CHANNEL_ERROR 0x18 // Not used
#define PWM_ADJUST_OK 0x19 // Not used
#define CRC_EOP 0x20 // 'SPACE'
#define STK_GET_SYNC 0x30 // '0'
#define STK_GET_SIGN_ON 0x31 // '1'
#define STK_SET_PARAMETER 0x40 // '@'
#define STK_GET_PARAMETER 0x41 // 'A'
#define STK_SET_DEVICE 0x42 // 'B'
#define STK_SET_DEVICE_EXT 0x45 // 'E'
#define STK_ENTER_PROGMODE 0x50 // 'P'
#define STK_LEAVE_PROGMODE 0x51 // 'Q'
#define STK_CHIP_ERASE 0x52 // 'R'
#define STK_CHECK_AUTOINC 0x53 // 'S'
#define STK_LOAD_ADDRESS 0x55 // 'U'
#define STK_UNIVERSAL 0x56 // 'V'
#define STK_PROG_FLASH 0x60 // '`'
#define STK_PROG_DATA 0x61 // 'a'
#define STK_PROG_FUSE 0x62 // 'b'
#define STK_PROG_LOCK 0x63 // 'c'
#define STK_PROG_PAGE 0x64 // 'd'
#define STK_PROG_FUSE_EXT 0x65 // 'e'
#define STK_READ_FLASH 0x70 // 'p'
#define STK_READ_DATA 0x71 // 'q'
#define STK_READ_FUSE 0x72 // 'r'
#define STK_READ_LOCK 0x73 // 's'
#define STK_READ_PAGE 0x74 // 't'
#define STK_READ_SIGN 0x75 // 'u'
#define STK_READ_OSCCAL 0x76 // 'v'
#define STK_READ_FUSE_EXT 0x77 // 'w'
#define STK_READ_OSCCAL_EXT 0x78 // 'x'

View file

@ -26,4 +26,5 @@ if(PYTHONINTERP_FOUND)
add_lua_export_target(x10 ${LUA_INCLUDES} -DPCBHORUS -DPCBX10) add_lua_export_target(x10 ${LUA_INCLUDES} -DPCBHORUS -DPCBX10)
add_lua_export_target(x12s ${LUA_INCLUDES} -DPCBHORUS -DPCBX12S) add_lua_export_target(x12s ${LUA_INCLUDES} -DPCBHORUS -DPCBX12S)
add_lua_export_target(t12 ${LUA_INCLUDES} -DPCBTARANIS -DPCBX7 -DRADIO_T12) add_lua_export_target(t12 ${LUA_INCLUDES} -DPCBTARANIS -DPCBX7 -DRADIO_T12)
add_lua_export_target(t16 ${LUA_INCLUDES} -DPCBHORUS -DPCBX10 -DRADIO_T16)
endif() endif()

View file

@ -27,6 +27,8 @@
#if defined(PCBX12S) #if defined(PCBX12S)
#include "lua/lua_exports_x12s.inc" // this line must be after lua headers #include "lua/lua_exports_x12s.inc" // this line must be after lua headers
#elif defined(RADIO_T16)
#include "lua/lua_exports_t16.inc"
#elif defined(PCBX10) #elif defined(PCBX10)
#include "lua/lua_exports_x10.inc" #include "lua/lua_exports_x10.inc"
#elif defined(PCBX9E) #elif defined(PCBX9E)

View file

@ -1512,7 +1512,7 @@ void doMixerCalculations()
static uint8_t countRangecheck = 0; static uint8_t countRangecheck = 0;
for (uint8_t i=0; i<NUM_MODULES; ++i) { for (uint8_t i=0; i<NUM_MODULES; ++i) {
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (moduleState[i].mode >= MODULE_MODE_BEEP_FIRST || multiModuleStatus.isBinding()) { if (moduleState[i].mode >= MODULE_MODE_BEEP_FIRST || getMultiModuleStatus(i).isBinding()) {
#else #else
if (moduleState[i].mode >= MODULE_MODE_BEEP_FIRST) { if (moduleState[i].mode >= MODULE_MODE_BEEP_FIRST) {
#endif #endif
@ -1862,7 +1862,7 @@ void opentxInit()
sportStopSendByteLoop(); sportStopSendByteLoop();
if (f_stat(AUTOUPDATE_FILENAME, nullptr) == FR_OK) { if (f_stat(AUTOUPDATE_FILENAME, nullptr) == FR_OK) {
FrSkyFirmwareInformation information; FrSkyFirmwareInformation information;
if (readFirmwareInformation(AUTOUPDATE_FILENAME, information) == nullptr) { if (readFrSkyFirmwareInformation(AUTOUPDATE_FILENAME, information) == nullptr) {
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
if (information.productFamily == FIRMWARE_FAMILY_BLUETOOTH_CHIP) { if (information.productFamily == FIRMWARE_FAMILY_BLUETOOTH_CHIP) {
if (bluetooth.flashFirmware(AUTOUPDATE_FILENAME) == nullptr) if (bluetooth.flashFirmware(AUTOUPDATE_FILENAME) == nullptr)

View file

@ -544,6 +544,8 @@ bool setTrimValue(uint8_t phase, uint8_t idx, int trim);
#if defined(PCBSKY9X) #if defined(PCBSKY9X)
#define ROTARY_ENCODER_GRANULARITY (2 << g_eeGeneral.rotarySteps) #define ROTARY_ENCODER_GRANULARITY (2 << g_eeGeneral.rotarySteps)
#elif defined(RADIO_T16)
#define ROTARY_ENCODER_GRANULARITY (1)
#else #else
#define ROTARY_ENCODER_GRANULARITY (2) #define ROTARY_ENCODER_GRANULARITY (2)
#endif #endif

View file

@ -56,6 +56,9 @@ static const char * options[] = {
#if defined(INTERNAL_MODULE_PPM) #if defined(INTERNAL_MODULE_PPM)
"internalppm", "internalppm",
#endif #endif
#if defined(INTERNAL_MODULE_MULTI)
"internalmulti",
#endif
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
"multimodule", "multimodule",
#endif #endif

View file

@ -34,7 +34,7 @@
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
inline bool isModuleMultimodule(uint8_t idx) inline bool isModuleMultimodule(uint8_t idx)
{ {
return idx == EXTERNAL_MODULE && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE; return g_model.moduleData[idx].type == MODULE_TYPE_MULTIMODULE;
} }
inline bool isModuleMultimoduleDSM2(uint8_t idx) inline bool isModuleMultimoduleDSM2(uint8_t idx)
@ -350,8 +350,10 @@ inline bool isModuleFailsafeAvailable(uint8_t idx)
return g_model.moduleData[idx].subType == MODULE_SUBTYPE_PXX1_ACCST_D16; return g_model.moduleData[idx].subType == MODULE_SUBTYPE_PXX1_ACCST_D16;
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (isModuleMultimodule(idx)) if (isModuleMultimodule(idx)){
return multiModuleStatus.isValid() && multiModuleStatus.supportsFailsafe(); MultiModuleStatus& status = getMultiModuleStatus(idx);
return status.isValid() && status.supportsFailsafe();
}
#endif #endif
if (isModuleR9M(idx)) if (isModuleR9M(idx))

View file

@ -31,27 +31,38 @@
#define MULTI_CHANS 16 #define MULTI_CHANS 16
#define MULTI_CHAN_BITS 11 #define MULTI_CHAN_BITS 11
static void sendFrameProtocolHeader(uint8_t port, bool failsafe); static void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe);
void sendChannels(uint8_t port); void sendChannels(uint8_t moduleIdx);
static void sendSetupFrame() static void sendMulti(uint8_t moduleIdx, uint8_t b)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE) {
intmodulePulsesData.multi.sendByte(b);
}
else
#endif
sendByteSbus(b);
}
static void sendSetupFrame(uint8_t moduleIdx)
{ {
// Old multi firmware will mark config messsages as invalid frame and throw them away // Old multi firmware will mark config messsages as invalid frame and throw them away
sendByteSbus('M'); sendMulti(moduleIdx, 'M');
sendByteSbus('P'); sendMulti(moduleIdx, 'P');
sendByteSbus(0x80); // Module Configuration sendMulti(moduleIdx, 0x80); // Module Configuration
sendByteSbus(1); // 1 byte data sendMulti(moduleIdx, 1); // 1 byte data
uint8_t config = 0x01 | 0x02; // inversion + multi_telemetry uint8_t config = 0x01 | 0x02; // inversion + multi_telemetry
#if !defined(PPM_PIN_SERIAL) #if !defined(PPM_PIN_SERIAL)
// TODO why PPM_PIN_SERIAL would change MULTI protocol? // TODO why PPM_PIN_SERIAL would change MULTI protocol?
config |= 0x04; // input synchronsisation config |= 0x04; // input synchronsisation
#endif #endif
sendByteSbus(config); sendMulti(moduleIdx, config);
} }
static void sendFailsafeChannels(uint8_t port) static void sendFailsafeChannels(uint8_t moduleIdx)
{ {
uint32_t bits = 0; uint32_t bits = 0;
uint8_t bitsavailable = 0; uint8_t bitsavailable = 0;
@ -60,61 +71,72 @@ static void sendFailsafeChannels(uint8_t port)
int16_t failsafeValue = g_model.failsafeChannels[i]; int16_t failsafeValue = g_model.failsafeChannels[i];
int pulseValue; int pulseValue;
if (g_model.moduleData[port].failsafeMode == FAILSAFE_HOLD) { if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_HOLD) {
pulseValue = 2047; pulseValue = 2047;
} }
else if (g_model.moduleData[port].failsafeMode == FAILSAFE_NOPULSES) { else if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_NOPULSES) {
pulseValue = 0; pulseValue = 0;
} }
else { else {
failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[port].channelsStart + i) - 2 * PPM_CENTER; failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[moduleIdx].channelsStart + i) - 2 * PPM_CENTER;
pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047); pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047);
} }
bits |= pulseValue << bitsavailable; bits |= pulseValue << bitsavailable;
bitsavailable += MULTI_CHAN_BITS; bitsavailable += MULTI_CHAN_BITS;
while (bitsavailable >= 8) { while (bitsavailable >= 8) {
sendByteSbus((uint8_t) (bits & 0xff)); sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
bits >>= 8; bits >>= 8;
bitsavailable -= 8; bitsavailable -= 8;
} }
} }
} }
void setupPulsesMultimodule() void setupPulsesMulti(uint8_t moduleIdx)
{ {
static int counter = 0; static int counter[2] = {0,0}; //TODO
// Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type)
counter[moduleIdx]++;
if (counter[moduleIdx] % 1000 == 500) {
sendSetupFrame(moduleIdx);
}
else if (counter[moduleIdx] % 1000 == 0 && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[moduleIdx].failsafeMode != FAILSAFE_RECEIVER) {
sendFrameProtocolHeader(moduleIdx, true);
sendFailsafeChannels(moduleIdx);
}
else {
// Normal Frame
sendFrameProtocolHeader(moduleIdx, false);
sendChannels(moduleIdx);
}
}
void setupPulsesMultiExternalModule()
{
#if defined(PPM_PIN_SERIAL) #if defined(PPM_PIN_SERIAL)
extmodulePulsesData.dsm2.serialByte = 0 ; extmodulePulsesData.dsm2.serialByte = 0 ;
extmodulePulsesData.dsm2.serialBitCount = 0 ; extmodulePulsesData.dsm2.serialBitCount = 0 ;
#else #else
extmodulePulsesData.dsm2.rest = multiSyncStatus.getAdjustedRefreshRate(); extmodulePulsesData.dsm2.rest = getMultiSyncStatus(EXTERNAL_MODULE).getAdjustedRefreshRate();
extmodulePulsesData.dsm2.index = 0; extmodulePulsesData.dsm2.index = 0;
#endif #endif
extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses; extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses;
// Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type) setupPulsesMulti(EXTERNAL_MODULE);
counter++;
if (counter % 1000 == 500) {
sendSetupFrame();
}
else if (counter % 1000 == 0 && g_model.moduleData[EXTERNAL_MODULE].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[EXTERNAL_MODULE].failsafeMode != FAILSAFE_RECEIVER) {
sendFrameProtocolHeader(EXTERNAL_MODULE, true);
sendFailsafeChannels(EXTERNAL_MODULE);
}
else {
// Normal Frame
sendFrameProtocolHeader(EXTERNAL_MODULE, false);
sendChannels(EXTERNAL_MODULE);
}
putDsm2Flush(); putDsm2Flush();
} }
#if defined(INTERNAL_MODULE_MULTI)
void setupPulsesMultiInternalModule()
{
intmodulePulsesData.multi.initFrame();
setupPulsesMulti(INTERNAL_MODULE);
}
#endif
void sendChannels(uint8_t port) void sendChannels(uint8_t moduleIdx)
{ {
uint32_t bits = 0; uint32_t bits = 0;
uint8_t bitsavailable = 0; uint8_t bitsavailable = 0;
@ -123,7 +145,7 @@ void sendChannels(uint8_t port)
// Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%] // Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
// Multi uses [204;1843] as [-100%;100%] // Multi uses [204;1843] as [-100%;100%]
for (int i = 0; i < MULTI_CHANS; i++) { for (int i = 0; i < MULTI_CHANS; i++) {
int channel = g_model.moduleData[port].channelsStart + i; int channel = g_model.moduleData[moduleIdx].channelsStart + i;
int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER; int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
// Scale to 80% // Scale to 80%
@ -133,36 +155,36 @@ void sendChannels(uint8_t port)
bits |= value << bitsavailable; bits |= value << bitsavailable;
bitsavailable += MULTI_CHAN_BITS; bitsavailable += MULTI_CHAN_BITS;
while (bitsavailable >= 8) { while (bitsavailable >= 8) {
sendByteSbus((uint8_t) (bits & 0xff)); sendMulti(moduleIdx, (uint8_t) (bits & 0xff));
bits >>= 8; bits >>= 8;
bitsavailable -= 8; bitsavailable -= 8;
} }
} }
} }
void sendFrameProtocolHeader(uint8_t port, bool failsafe) void sendFrameProtocolHeader(uint8_t moduleIdx, bool failsafe)
{// byte 1+2, protocol information {// byte 1+2, protocol information
// Our enumeration starts at 0 // Our enumeration starts at 0
int type = g_model.moduleData[port].getMultiProtocol(false) + 1; int type = g_model.moduleData[moduleIdx].getMultiProtocol(false) + 1;
int subtype = g_model.moduleData[port].subType; int subtype = g_model.moduleData[moduleIdx].subType;
int8_t optionValue = g_model.moduleData[port].multi.optionValue; int8_t optionValue = g_model.moduleData[moduleIdx].multi.optionValue;
uint8_t protoByte = 0; uint8_t protoByte = 0;
if (moduleState[port].mode == MODULE_MODE_BIND) if (moduleState[moduleIdx].mode == MODULE_MODE_BIND)
protoByte |= MULTI_SEND_BIND; protoByte |= MULTI_SEND_BIND;
else if (moduleState[port].mode == MODULE_MODE_RANGECHECK) else if (moduleState[moduleIdx].mode == MODULE_MODE_RANGECHECK)
protoByte |= MULTI_SEND_RANGECHECK; protoByte |= MULTI_SEND_RANGECHECK;
// rfProtocol // rfProtocol
if (g_model.moduleData[port].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) { if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2) {
// Autobinding should always be done in DSMX 11ms // Autobinding should always be done in DSMX 11ms
if (g_model.moduleData[port].multi.autoBindMode && moduleState[port].mode == MODULE_MODE_BIND) if (g_model.moduleData[moduleIdx].multi.autoBindMode && moduleState[moduleIdx].mode == MODULE_MODE_BIND)
subtype = MM_RF_DSM2_SUBTYPE_AUTO; subtype = MM_RF_DSM2_SUBTYPE_AUTO;
// Multi module in DSM mode wants the number of channels to be used as option value // Multi module in DSM mode wants the number of channels to be used as option value
optionValue = sentModuleChannels(EXTERNAL_MODULE); optionValue = sentModuleChannels(moduleIdx);
} }
@ -175,7 +197,7 @@ void sendFrameProtocolHeader(uint8_t port, bool failsafe)
if (type >= 25) if (type >= 25)
type = type + 1; type = type + 1;
if (g_model.moduleData[port].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_FRSKY) { if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_FRSKY) {
if (subtype == MM_RF_FRSKY_SUBTYPE_D8) { if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
//D8 //D8
type = 3; type = 3;
@ -199,12 +221,12 @@ void sendFrameProtocolHeader(uint8_t port, bool failsafe)
// Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead // Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead
// of sending Frsky D telemetry // of sending Frsky D telemetry
if (g_model.moduleData[port].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A) if (g_model.moduleData[moduleIdx].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
optionValue = optionValue | 0x80; optionValue = optionValue | 0x80;
// For custom protocol send unmodified type byte // For custom protocol send unmodified type byte
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_CUSTOM_SELECTED) if (g_model.moduleData[moduleIdx].getMultiProtocol(true) == MM_RF_CUSTOM_SELECTED)
type = g_model.moduleData[port].getMultiProtocol(false); type = g_model.moduleData[moduleIdx].getMultiProtocol(false);
uint8_t headerByte = 0x54; uint8_t headerByte = 0x54;
@ -213,24 +235,24 @@ void sendFrameProtocolHeader(uint8_t port, bool failsafe)
// header, byte 0, 0x55 for proto 0-31 0x54 for 32-63 // header, byte 0, 0x55 for proto 0-31 0x54 for 32-63
if (type <= 31) if (type <= 31)
sendByteSbus(headerByte+1); sendMulti(moduleIdx, headerByte+1);
else else
sendByteSbus(headerByte); sendMulti(moduleIdx, headerByte);
// protocol byte // protocol byte
protoByte |= (type & 0x1f); protoByte |= (type & 0x1f);
if (g_model.moduleData[port].getMultiProtocol(true) != MODULE_SUBTYPE_MULTI_DSM2) if (g_model.moduleData[moduleIdx].getMultiProtocol(true) != MODULE_SUBTYPE_MULTI_DSM2)
protoByte |= (g_model.moduleData[port].multi.autoBindMode << 6); protoByte |= (g_model.moduleData[moduleIdx].multi.autoBindMode << 6);
sendByteSbus(protoByte); sendMulti(moduleIdx, protoByte);
// byte 2, subtype, powermode, model id // byte 2, subtype, powermode, model id
sendByteSbus((uint8_t) ((g_model.header.modelId[port] & 0x0f) sendMulti(moduleIdx, (uint8_t) ((g_model.header.modelId[moduleIdx] & 0x0f)
| ((subtype & 0x7) << 4) | ((subtype & 0x7) << 4)
| (g_model.moduleData[port].multi.lowPowerMode << 7)) | (g_model.moduleData[moduleIdx].multi.lowPowerMode << 7))
); );
// byte 3 // byte 3
sendByteSbus((uint8_t) optionValue); sendMulti(moduleIdx, (uint8_t) optionValue);
} }

41
radio/src/pulses/multi.h Normal file
View file

@ -0,0 +1,41 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* 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.
*/
#ifndef _MULTI_PULSES_H_
#define _MULTI_PULSES_H_
#include "pulses_common.h"
class UartMultiPulses: public DataBuffer<uint8_t, 64>
{
public:
void initFrame()
{
initBuffer();
}
void sendByte(uint8_t b)
{
if (getSize() < 64)
*ptr++ = b;
}
};
#endif

View file

@ -269,7 +269,7 @@ void setupPulsesExternalModule(uint8_t protocol)
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
case PROTOCOL_CHANNELS_MULTIMODULE: case PROTOCOL_CHANNELS_MULTIMODULE:
setupPulsesMultimodule(); setupPulsesMultiExternalModule();
scheduleNextMixerCalculation(EXTERNAL_MODULE, MULTIMODULE_PERIOD); scheduleNextMixerCalculation(EXTERNAL_MODULE, MULTIMODULE_PERIOD);
break; break;
#endif #endif
@ -306,10 +306,17 @@ void enablePulsesInternalModule(uint8_t protocol)
#if defined(PXX2) #if defined(PXX2)
case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: case PROTOCOL_CHANNELS_PXX2_HIGHSPEED:
intmoduleSerialStart(PXX2_HIGHSPEED_BAUDRATE, true); intmoduleSerialStart(PXX2_HIGHSPEED_BAUDRATE, true, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b);
break; break;
#endif #endif
#if defined(INTERNAL_MODULE_MULTI)
case PROTOCOL_CHANNELS_MULTIMODULE:
intmodulePulsesData.multi.initFrame();
intmoduleSerialStart(MULTIMODULE_BAUDRATE, true, USART_Parity_Even, USART_StopBits_2, USART_WordLength_9b);
intmoduleTimerStart(MULTIMODULE_PERIOD);
break;
#endif
default: default:
break; break;
} }
@ -355,6 +362,13 @@ void setupPulsesInternalModule(uint8_t protocol)
break; break;
#endif #endif
#if defined(INTERNAL_MODULE_MULTI)
case PROTOCOL_CHANNELS_MULTIMODULE:
setupPulsesMultiInternalModule();
scheduleNextMixerCalculation(INTERNAL_MODULE, MULTIMODULE_PERIOD);
break;
#endif
default: default:
break; break;
} }

View file

@ -24,8 +24,9 @@
#include "definitions.h" #include "definitions.h"
#include "dataconstants.h" #include "dataconstants.h"
#include "pulses_common.h" #include "pulses_common.h"
#include "pulses/pxx1.h" #include "pxx1.h"
#include "pulses/pxx2.h" #include "pxx2.h"
#include "multi.h"
#include "modules_helpers.h" #include "modules_helpers.h"
#include "ff.h" #include "ff.h"
@ -248,6 +249,10 @@ union InternalModulePulsesData {
Pxx2Pulses pxx2; Pxx2Pulses pxx2;
#endif #endif
#if defined(INTERNAL_MODULE_MULTI) //&& defined(INTMODULE_USART)
UartMultiPulses multi;
#endif
#if defined(INTERNAL_MODULE_PPM) #if defined(INTERNAL_MODULE_PPM)
PpmPulsesData<pulse_duration_t> ppm; PpmPulsesData<pulse_duration_t> ppm;
#endif #endif
@ -294,11 +299,14 @@ union TrainerPulsesData {
extern TrainerPulsesData trainerPulsesData; extern TrainerPulsesData trainerPulsesData;
#if defined(HARDWARE_INTERNAL_MODULE)
bool setupPulsesInternalModule(); bool setupPulsesInternalModule();
#endif
bool setupPulsesExternalModule(); bool setupPulsesExternalModule();
void setupPulsesDSM2(); void setupPulsesDSM2();
void setupPulsesCrossfire(); void setupPulsesCrossfire();
void setupPulsesMultimodule(); void setupPulsesMultiExternalModule();
void setupPulsesMultiInternalModule();
void setupPulsesSbus(); void setupPulsesSbus();
void setupPulsesPPMInternalModule(); void setupPulsesPPMInternalModule();
void setupPulsesPPMExternalModule(); void setupPulsesPPMExternalModule();

View file

@ -469,7 +469,7 @@ const char * Pxx2OtaUpdate::doFlashFirmware(const char * filename)
uint32_t size; uint32_t size;
const char * ext = getFileExtension(filename); const char * ext = getFileExtension(filename);
if (ext && !strcasecmp(ext, UPDATE_FIRMWARE_EXT)) { if (ext && !strcasecmp(ext, FRSKY_FIRMWARE_EXT)) {
FrSkyFirmwareInformation * information = (FrSkyFirmwareInformation *) buffer; FrSkyFirmwareInformation * information = (FrSkyFirmwareInformation *) buffer;
if (f_read(&file, buffer, sizeof(FrSkyFirmwareInformation), &count) != FR_OK || count != sizeof(FrSkyFirmwareInformation)) { if (f_read(&file, buffer, sizeof(FrSkyFirmwareInformation), &count) != FR_OK || count != sizeof(FrSkyFirmwareInformation)) {
f_close(&file); f_close(&file);

View file

@ -70,7 +70,8 @@ const char RADIO_SETTINGS_PATH[] = RADIO_PATH "/radio.bin";
#define FIRMWARE_EXT ".bin" #define FIRMWARE_EXT ".bin"
#define EEPROM_EXT ".bin" #define EEPROM_EXT ".bin"
#define SPORT_FIRMWARE_EXT ".frk" #define SPORT_FIRMWARE_EXT ".frk"
#define UPDATE_FIRMWARE_EXT ".frsk" #define FRSKY_FIRMWARE_EXT ".frsk"
#define MULTI_FIRMWARE_EXT ".bin"
#define LEN_FILE_EXTENSION_MAX 5 // longest used, including the dot, excluding null term. #define LEN_FILE_EXTENSION_MAX 5 // longest used, including the dot, excluding null term.
@ -125,6 +126,8 @@ const char * getBasename(const char * path);
#if defined(PCBX12S) #if defined(PCBX12S)
#define OTX_FOURCC 0x3478746F // otx for X12S #define OTX_FOURCC 0x3478746F // otx for X12S
#elif defined(RADIO_T16)
#define OTX_FOURCC 0x3F78746F // otx for Jumper T16
#elif defined(PCBX10) #elif defined(PCBX10)
#define OTX_FOURCC 0x3778746F // otx for X10 #define OTX_FOURCC 0x3778746F // otx for X10
#elif defined(PCBX9E) #elif defined(PCBX9E)

View file

@ -124,9 +124,11 @@ void convertModelData_218_to_219(ModelData &model)
memclear(&newModel.header.bitmap[10], 4); memclear(&newModel.header.bitmap[10], 4);
memcpy(newModel.timers, oldModel.timers, offsetof(ModelData_v218, mixData) - offsetof(ModelData_v218, timers)); memcpy(newModel.timers, oldModel.timers, offsetof(ModelData_v218, mixData) - offsetof(ModelData_v218, timers));
#if defined(BLUETOOTH)
// trainer battery compartment removed // trainer battery compartment removed
if (newModel.trainerData.mode >= TRAINER_MODE_MASTER_BLUETOOTH) if (newModel.trainerData.mode >= TRAINER_MODE_MASTER_BLUETOOTH)
newModel.trainerData.mode -= 1; newModel.trainerData.mode -= 1;
#endif
#endif #endif
memclear(newModel.mixData, sizeof(ModelData_v219) - offsetof(ModelData_v219, mixData)); memclear(newModel.mixData, sizeof(ModelData_v219) - offsetof(ModelData_v219, mixData));

View file

@ -143,7 +143,7 @@ endif()
if(MULTIMODULE) if(MULTIMODULE)
add_definitions(-DMULTIMODULE) add_definitions(-DMULTIMODULE)
set(SRC ${SRC} pulses/multi.cpp telemetry/spektrum.cpp telemetry/flysky_ibus.cpp telemetry/multi.cpp) set(SRC ${SRC} pulses/multi.cpp telemetry/spektrum.cpp telemetry/flysky_ibus.cpp telemetry/multi.cpp io/multi_firmware_update.cpp)
endif() endif()
if(CROSSFIRE) if(CROSSFIRE)

View file

@ -22,8 +22,10 @@
#if defined(SIMU) #if defined(SIMU)
// not needed // not needed
#elif defined(RADIO_T16)
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, 1,1,1, -1,1, -1,1};
#elif defined(PCBX10) #elif defined(PCBX10)
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,1,-1, 1,1, 1, -1}; const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,1,-1, 1,1, 1, -1};
#elif defined(PCBX9E) #elif defined(PCBX9E)
#if defined(HORUS_STICKS) #if defined(HORUS_STICKS)
const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,-1,-1,1, -1,1,-1, -1,-1,-1}; const int8_t adcDirection[NUM_ANALOGS] = {1,-1,1,-1, -1,-1,-1,1, -1,1,-1, -1,-1,-1};

View file

@ -95,8 +95,14 @@ void audioConsumeCurrentBuffer()
nextBuffer = audioQueue.buffersFifo.getNextFilledBuffer(); nextBuffer = audioQueue.buffersFifo.getNextFilledBuffer();
if (nextBuffer) { if (nextBuffer) {
#if defined(AUDIO_MUTE_GPIO_PIN) #if defined(AUDIO_MUTE_GPIO_PIN)
// un-mute // if muted
GPIO_ResetBits(AUDIO_MUTE_GPIO, AUDIO_MUTE_GPIO_PIN); if (GPIO_ReadOutputDataBit(AUDIO_MUTE_GPIO, AUDIO_MUTE_GPIO_PIN)) {
// ..un-mute
GPIO_ResetBits(AUDIO_MUTE_GPIO, AUDIO_MUTE_GPIO_PIN);
#if defined(AUDIO_UNMUTE_DELAY)
RTOS_WAIT_MS(AUDIO_UNMUTE_DELAY);
#endif
}
#endif #endif
AUDIO_DMA_Stream->CR &= ~DMA_SxCR_EN ; // Disable DMA channel AUDIO_DMA_Stream->CR &= ~DMA_SxCR_EN ; // Disable DMA channel
AUDIO_DMA->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5 | DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5 ; // Write ones to clear bits AUDIO_DMA->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5 | DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5 ; // Write ones to clear bits

View file

@ -20,6 +20,7 @@
#include "opentx.h" #include "opentx.h"
#if defined(AUX_SERIAL)
uint8_t auxSerialMode = 0; uint8_t auxSerialMode = 0;
Fifo<uint8_t, 512> auxSerialTxFifo; Fifo<uint8_t, 512> auxSerialTxFifo;
AuxSerialRxFifo auxSerialRxFifo __DMA (AUX_SERIAL_DMA_Stream_RX); AuxSerialRxFifo auxSerialRxFifo __DMA (AUX_SERIAL_DMA_Stream_RX);
@ -183,4 +184,5 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void)
} }
#endif #endif
} }
#endif #endif
#endif // AUX_SERIAL

View file

@ -39,14 +39,20 @@ void intmoduleStop()
USART_DeInit(INTMODULE_USART); USART_DeInit(INTMODULE_USART);
GPIO_ResetBits(INTMODULE_GPIO, INTMODULE_TX_GPIO_PIN | INTMODULE_RX_GPIO_PIN); GPIO_ResetBits(INTMODULE_GPIO, INTMODULE_TX_GPIO_PIN | INTMODULE_RX_GPIO_PIN);
#if defined(INTERNAL_MODULE_MULTI)
// stop pulses timer
INTMODULE_TIMER->DIER &= ~TIM_DIER_CC2IE;
INTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN;
#endif
} }
void intmodulePxx1SerialStart() void intmodulePxx1SerialStart()
{ {
intmoduleSerialStart(INTMODULE_PXX1_SERIAL_BAUDRATE, false); intmoduleSerialStart(INTMODULE_PXX1_SERIAL_BAUDRATE, false, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b);
} }
void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable) void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable, uint16_t parity, uint16_t stopBits, uint16_t wordLength)
{ {
INTERNAL_MODULE_ON(); INTERNAL_MODULE_ON();
@ -71,9 +77,9 @@ void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable)
USART_DeInit(INTMODULE_USART); USART_DeInit(INTMODULE_USART);
USART_InitTypeDef USART_InitStructure; USART_InitTypeDef USART_InitStructure;
USART_InitStructure.USART_BaudRate = baudrate; USART_InitStructure.USART_BaudRate = baudrate;
USART_InitStructure.USART_Parity = USART_Parity_No; USART_InitStructure.USART_Parity = parity;
USART_InitStructure.USART_StopBits = USART_StopBits_1; USART_InitStructure.USART_StopBits = stopBits;
USART_InitStructure.USART_WordLength = USART_WordLength_8b; USART_InitStructure.USART_WordLength = wordLength;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
USART_Init(INTMODULE_USART, &USART_InitStructure); USART_Init(INTMODULE_USART, &USART_InitStructure);
@ -87,6 +93,28 @@ void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable)
} }
} }
#if defined(INTERNAL_MODULE_MULTI)
void intmoduleTimerStart(uint32_t periodMs)
{
// Timer
INTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN;
INTMODULE_TIMER->PSC = INTMODULE_TIMER_FREQ / 2000000 - 1; // 0.5uS (2Mhz)
INTMODULE_TIMER->ARR = periodMs * 2000;
INTMODULE_TIMER->CCR2 = (periodMs - 1) * 2000;
INTMODULE_TIMER->CCER = TIM_CCER_CC3E;
INTMODULE_TIMER->CCMR2 = 0;
INTMODULE_TIMER->EGR = 1; // Restart
INTMODULE_TIMER->CCMR2 = TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_0; // Toggle CC1 o/p
INTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag
INTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt
INTMODULE_TIMER->CR1 |= TIM_CR1_CEN;
NVIC_EnableIRQ(INTMODULE_TIMER_IRQn);
NVIC_SetPriority(INTMODULE_TIMER_IRQn, 7);
}
#endif
#define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE) #define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE)
extern "C" void INTMODULE_USART_IRQHandler(void) extern "C" void INTMODULE_USART_IRQHandler(void)
{ {
@ -98,7 +126,6 @@ extern "C" void INTMODULE_USART_IRQHandler(void)
intmoduleFifo.errors++; intmoduleFifo.errors++;
} }
else { else {
hardwareOptions.pxx2Enabled = true;
intmoduleFifo.push(data); intmoduleFifo.push(data);
} }
status = INTMODULE_USART->SR; status = INTMODULE_USART->SR;
@ -152,5 +179,20 @@ void intmoduleSendNextFrame()
intmoduleSendBuffer(intmodulePulsesData.pxx_uart.getData(), intmodulePulsesData.pxx_uart.getSize()); intmoduleSendBuffer(intmodulePulsesData.pxx_uart.getData(), intmodulePulsesData.pxx_uart.getSize());
break; break;
#endif #endif
#if defined(INTERNAL_MODULE_MULTI)
case PROTOCOL_CHANNELS_MULTIMODULE:
intmoduleSendBuffer(intmodulePulsesData.multi.getData(), intmodulePulsesData.multi.getSize());
break;
#endif
} }
} }
#if defined(INTERNAL_MODULE_MULTI)
extern "C" void INTMODULE_TIMER_IRQHandler()
{
INTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // clear flag
setupPulsesInternalModule();
intmoduleSendNextFrame();
}
#endif

View file

@ -71,6 +71,27 @@ void rotaryEncoderInit()
void rotaryEncoderCheck() void rotaryEncoderCheck()
{ {
#if defined(RADIO_T16)
static uint8_t state = 0;
uint8_t pins = ROTARY_ENCODER_POSITION();
if (pins != (state & 0x03) && !(readKeys() & (1 << KEY_ENTER))) {
if ((pins & 0x01) ^ ((pins & 0x02) >> 1)) {
if ((state & 0x03) == 3)
++rotencValue;
else
--rotencValue;
}
else
{
if ((state & 0x03) == 3)
--rotencValue;
else if ((state & 0x03) == 0)
++rotencValue;
}
state &= ~0x03 ;
state |= pins ;
#else
uint8_t newPosition = ROTARY_ENCODER_POSITION(); uint8_t newPosition = ROTARY_ENCODER_POSITION();
if (newPosition != rotencPosition && !(readKeys() & (1 << KEY_ENTER))) { if (newPosition != rotencPosition && !(readKeys() & (1 << KEY_ENTER))) {
if ((rotencPosition & 0x01) ^ ((newPosition & 0x02) >> 1)) { if ((rotencPosition & 0x01) ^ ((newPosition & 0x02) >> 1)) {
@ -80,6 +101,7 @@ void rotaryEncoderCheck()
++rotencValue; ++rotencValue;
} }
rotencPosition = newPosition; rotencPosition = newPosition;
#endif
#if !defined(BOOT) #if !defined(BOOT)
if (g_eeGeneral.backlightMode & e_backlight_mode_keys) { if (g_eeGeneral.backlightMode & e_backlight_mode_keys) {
backlightOn(); backlightOn();
@ -127,4 +149,4 @@ extern "C" void ROTARY_ENCODER_TIMER_IRQHandler(void)
ROTARY_ENCODER_TIMER->SR &= ~TIM_SR_UIF; ROTARY_ENCODER_TIMER->SR &= ~TIM_SR_UIF;
ROTARY_ENCODER_TIMER->CR1 = 0; ROTARY_ENCODER_TIMER->CR1 = 0;
rotaryEncoderCheck(); rotaryEncoderCheck();
} }

View file

@ -44,15 +44,23 @@ if (PCB STREQUAL X10)
set(FLAVOUR x10express) set(FLAVOUR x10express)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF) option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON) option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON)
set(BLUETOOTH ON)
elseif (PCBREV STREQUAL T16)
set(FLAVOUR t16)
set(LUA_EXPORT lua_export_t16)
add_definitions(-DRADIO_T16)
option(INTERNAL_MODULE_MULTI "Support for MULTI internal module" OFF)
else() else()
set(FLAVOUR x10) set(FLAVOUR x10)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON) option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF) option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(BLUETOOTH ON)
endif() endif()
elseif (PCB STREQUAL X12S) elseif (PCB STREQUAL X12S)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON) option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF) option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(FLAVOUR x12s) set(FLAVOUR x12s)
set(BLUETOOTH ON)
set(PCBREV "13" CACHE STRING "PCB Revision") set(PCBREV "13" CACHE STRING "PCB Revision")
if(${PCBREV} GREATER 10) if(${PCBREV} GREATER 10)
option(INTERNAL_GPS "Internal GPS installed" YES) option(INTERNAL_GPS "Internal GPS installed" YES)
@ -91,14 +99,21 @@ if(NOT UNEXPECTED_SHUTDOWN)
endif() endif()
if(INTERNAL_MODULE_PXX1) if(INTERNAL_MODULE_PXX1)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_PXX1) add_definitions(-DINTERNAL_MODULE_PXX1)
endif() endif()
if(INTERNAL_MODULE_PXX2) if(INTERNAL_MODULE_PXX2)
set(PXX2 ON) set(PXX2 ON)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_PXX2) add_definitions(-DINTERNAL_MODULE_PXX2)
endif() endif()
if(INTERNAL_MODULE_MULTI)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_MULTI)
endif()
include_directories(${RADIO_SRC_DIRECTORY}/fonts/480x272 gui/${GUI_DIR} gui/${GUI_DIR}/layouts) include_directories(${RADIO_SRC_DIRECTORY}/fonts/480x272 gui/${GUI_DIR} gui/${GUI_DIR}/layouts)
file(GLOB THEMES_SRC RELATIVE ${RADIO_SRC_DIRECTORY}/gui/480x272 ${RADIO_SRC_DIRECTORY}/gui/480x272/themes/*.cpp) file(GLOB THEMES_SRC RELATIVE ${RADIO_SRC_DIRECTORY}/gui/480x272 ${RADIO_SRC_DIRECTORY}/gui/480x272/themes/*.cpp)
@ -163,10 +178,15 @@ set(TARGET_SRC
../common/arm/stm32/heartbeat_driver.cpp ../common/arm/stm32/heartbeat_driver.cpp
../common/arm/stm32/timers_driver.cpp ../common/arm/stm32/timers_driver.cpp
../common/arm/stm32/intmodule_serial_driver.cpp ../common/arm/stm32/intmodule_serial_driver.cpp
../common/arm/stm32/bluetooth_driver.cpp
../common/arm/stm32/rotary_encoder_driver.cpp ../common/arm/stm32/rotary_encoder_driver.cpp
) )
if(BLUETOOTH)
add_definitions(-DBLUETOOTH)
set(TARGET_SRC ${TARGET_SRC} ../common/arm/stm32/bluetooth_driver.cpp)
set(SRC ${SRC} bluetooth.cpp)
endif()
set(FIRMWARE_TARGET_SRC set(FIRMWARE_TARGET_SRC
${FIRMWARE_TARGET_SRC} ${FIRMWARE_TARGET_SRC}
${LCD_DRIVER} ${LCD_DRIVER}
@ -179,12 +199,10 @@ set(FIRMWARE_TARGET_SRC
../common/arm/loadboot.cpp ../common/arm/loadboot.cpp
) )
add_definitions(-DBLUETOOTH)
set(SRC set(SRC
${SRC} ${SRC}
bluetooth.cpp
io/frsky_firmware_update.cpp io/frsky_firmware_update.cpp
io/multi_firmware_update.cpp
) )
set(STM32LIB_SRC set(STM32LIB_SRC
@ -202,4 +220,3 @@ if(PYTHONINTERP_FOUND)
DEPENDS ${RADIO_DIRECTORY}/src/datastructs.h ${RADIO_DIRECTORY}/util/generate_datacopy.py DEPENDS ${RADIO_DIRECTORY}/src/datastructs.h ${RADIO_DIRECTORY}/util/generate_datacopy.py
) )
endif() endif()

View file

@ -59,7 +59,7 @@ extern uint16_t sessionTimer;
#define SLAVE_MODE() (g_model.trainerData.mode == TRAINER_MODE_SLAVE) #define SLAVE_MODE() (g_model.trainerData.mode == TRAINER_MODE_SLAVE)
#if defined(PCBX10) #if defined(PCBX10) && !defined(RADIO_T16)
#define TRAINER_CONNECTED() (GPIO_ReadInputDataBit(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN) == Bit_SET) #define TRAINER_CONNECTED() (GPIO_ReadInputDataBit(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN) == Bit_SET)
#else #else
#define TRAINER_CONNECTED() (GPIO_ReadInputDataBit(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN) == Bit_RESET) #define TRAINER_CONNECTED() (GPIO_ReadInputDataBit(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN) == Bit_RESET)
@ -173,7 +173,10 @@ void SDRAM_Init();
void init_intmodule_heartbeat(); void init_intmodule_heartbeat();
void check_intmodule_heartbeat(); void check_intmodule_heartbeat();
void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable); void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable, uint16_t parity, uint16_t stopBits, uint16_t wordLength);
#if defined(INTERNAL_MODULE_MULTI)
void intmoduleTimerStart(uint32_t periodMs);
#endif
void intmoduleSendByte(uint8_t byte); void intmoduleSendByte(uint8_t byte);
void intmoduleSendBuffer(const uint8_t * data, uint8_t size); void intmoduleSendBuffer(const uint8_t * data, uint8_t size);
void intmoduleSendNextFrame(); void intmoduleSendNextFrame();
@ -182,6 +185,7 @@ void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inver
void extmoduleInvertedSerialStart(uint32_t baudrate); void extmoduleInvertedSerialStart(uint32_t baudrate);
void extmoduleSendBuffer(const uint8_t * data, uint8_t size); void extmoduleSendBuffer(const uint8_t * data, uint8_t size);
void extmoduleSendNextFrame(); void extmoduleSendNextFrame();
void extmoduleSendInvertedByte(uint8_t byte);
// Trainer driver // Trainer driver
void init_trainer_ppm(); void init_trainer_ppm();
@ -507,6 +511,8 @@ void backlightEnable(uint8_t dutyCycle = 0);
#define BACKLIGHT_LEVEL_MAX 100 #define BACKLIGHT_LEVEL_MAX 100
#if defined(PCBX12S) #if defined(PCBX12S)
#define BACKLIGHT_LEVEL_MIN 5 #define BACKLIGHT_LEVEL_MIN 5
#elif defined(RADIO_T16)
#define BACKLIGHT_LEVEL_MIN 1
#else #else
#define BACKLIGHT_LEVEL_MIN 46 #define BACKLIGHT_LEVEL_MIN 46
#endif #endif
@ -521,6 +527,10 @@ void usbJoystickUpdate();
#define USB_NAME "FrSky Horus" #define USB_NAME "FrSky Horus"
#define USB_MANUFACTURER 'F', 'r', 'S', 'k', 'y', ' ', ' ', ' ' /* 8 bytes */ #define USB_MANUFACTURER 'F', 'r', 'S', 'k', 'y', ' ', ' ', ' ' /* 8 bytes */
#define USB_PRODUCT 'H', 'o', 'r', 'u', 's', ' ', ' ', ' ' /* 8 Bytes */ #define USB_PRODUCT 'H', 'o', 'r', 'u', 's', ' ', ' ', ' ' /* 8 Bytes */
#elif defined(RADIO_T16)
#define USB_NAME "Jumper T16"
#define USB_MANUFACTURER 'J', 'u', 'm', 'p', 'e', 'r', ' ', ' ' /* 8 bytes */
#define USB_PRODUCT 'T', '1', '6', ' ', ' ', ' ', ' ', ' ' /* 8 Bytes */
#elif defined(PCBX10) #elif defined(PCBX10)
#define USB_NAME "FrSky X10" #define USB_NAME "FrSky X10"
#define USB_MANUFACTURER 'F', 'r', 'S', 'k', 'y', ' ', ' ', ' ' /* 8 bytes */ #define USB_MANUFACTURER 'F', 'r', 'S', 'k', 'y', ' ', ' ', ' ' /* 8 bytes */
@ -554,7 +564,7 @@ void telemetryPortSetDirectionInput();
void telemetryPortSetDirectionOutput(); void telemetryPortSetDirectionOutput();
void sportSendByte(uint8_t byte); void sportSendByte(uint8_t byte);
void sportSendBuffer(const uint8_t * buffer, uint32_t count); void sportSendBuffer(const uint8_t * buffer, uint32_t count);
uint8_t telemetryGetByte(uint8_t * byte); bool telemetryGetByte(uint8_t * byte);
void telemetryClearFifo(); void telemetryClearFifo();
extern uint32_t telemetryErrors; extern uint32_t telemetryErrors;
@ -605,6 +615,7 @@ void bluetoothDisable();
extern DMAFifo<512> telemetryFifo; extern DMAFifo<512> telemetryFifo;
typedef DMAFifo<32> AuxSerialRxFifo; typedef DMAFifo<32> AuxSerialRxFifo;
extern AuxSerialRxFifo auxSerialRxFifo; extern AuxSerialRxFifo auxSerialRxFifo;
extern volatile uint32_t externalModulePort;
#endif #endif
#endif // _BOARD_H_ #endif // _BOARD_H_

View file

@ -350,6 +350,38 @@ void extmoduleSendNextFrame()
} }
} }
void extmoduleSendInvertedByte(uint8_t byte)
{
uint16_t time;
uint32_t i;
__disable_irq();
time = getTmr2MHz();
GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
while ((uint16_t) (getTmr2MHz() - time) < 34) {
// wait
}
time += 34;
for (i = 0 ; i < 8 ; i += 1) {
if (byte & 1) {
GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
}
else {
GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
}
byte >>= 1 ;
while ((uint16_t) (getTmr2MHz() - time) < 35) {
// wait
}
time += 35 ;
}
GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
__enable_irq() ; // No need to wait for the stop bit to complete
while ((uint16_t) (getTmr2MHz() - time) < 34) {
// wait
}
}
extern "C" void EXTMODULE_TIMER_DMA_IRQHandler() extern "C" void EXTMODULE_TIMER_DMA_IRQHandler()
{ {
if (!DMA_GetITStatus(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC)) if (!DMA_GetITStatus(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC))

View file

@ -183,10 +183,17 @@
#define TRIMS_GPIO_PIN_RVU GPIO_Pin_12 // PJ.12 #define TRIMS_GPIO_PIN_RVU GPIO_Pin_12 // PJ.12
#define TRIMS_GPIO_REG_RHR GPIOD->IDR #define TRIMS_GPIO_REG_RHR GPIOD->IDR
#define TRIMS_GPIO_PIN_RHR GPIO_Pin_7 // PD.07 #define TRIMS_GPIO_PIN_RHR GPIO_Pin_7 // PD.07
#define TRIMS_GPIO_REG_LSU GPIOJ->IDR #if defined(RADIO_T16)
#define TRIMS_GPIO_PIN_LSU GPIO_Pin_8 // PJ.08 #define TRIMS_GPIO_REG_LSU GPIOD->IDR
#define TRIMS_GPIO_REG_LSD GPIOD->IDR #define TRIMS_GPIO_PIN_LSU GPIO_Pin_13 // PD.13
#define TRIMS_GPIO_PIN_LSD GPIO_Pin_13 // PD.13 #define TRIMS_GPIO_REG_LSD GPIOJ->IDR
#define TRIMS_GPIO_PIN_LSD GPIO_Pin_8 // PJ.08
#else
#define TRIMS_GPIO_REG_LSU GPIOJ->IDR
#define TRIMS_GPIO_PIN_LSU GPIO_Pin_8 // PJ.08
#define TRIMS_GPIO_REG_LSD GPIOD->IDR
#define TRIMS_GPIO_PIN_LSD GPIO_Pin_13 // PD.13
#endif
#define TRIMS_GPIO_REG_RSU GPIOB->IDR #define TRIMS_GPIO_REG_RSU GPIOB->IDR
#define TRIMS_GPIO_PIN_RSU GPIO_Pin_14 // PB.14 #define TRIMS_GPIO_PIN_RSU GPIO_Pin_14 // PB.14
#define TRIMS_GPIO_REG_RSD GPIOB->IDR #define TRIMS_GPIO_REG_RSD GPIOB->IDR
@ -281,8 +288,12 @@
#define ADC_DMA_Stream DMA2_Stream0 #define ADC_DMA_Stream DMA2_Stream0
#define ADC_SET_DMA_FLAGS() ADC_DMA->LIFCR = (DMA_LIFCR_CTCIF0 | DMA_LIFCR_CHTIF0 | DMA_LIFCR_CTEIF0 | DMA_LIFCR_CDMEIF0 | DMA_LIFCR_CFEIF0) #define ADC_SET_DMA_FLAGS() ADC_DMA->LIFCR = (DMA_LIFCR_CTCIF0 | DMA_LIFCR_CHTIF0 | DMA_LIFCR_CTEIF0 | DMA_LIFCR_CDMEIF0 | DMA_LIFCR_CFEIF0)
#define ADC_TRANSFER_COMPLETE() (ADC_DMA->LISR & DMA_LISR_TCIF0) #define ADC_TRANSFER_COMPLETE() (ADC_DMA->LISR & DMA_LISR_TCIF0)
#if defined(RADIO_T16)
#define ADC_VREF_PREC2 300
#else
#define ADC_VREF_PREC2 250 #define ADC_VREF_PREC2 250
#endif #endif
#endif
// Power // Power
#define PWR_RCC_AHB1Periph RCC_AHB1Periph_GPIOJ #define PWR_RCC_AHB1Periph RCC_AHB1Periph_GPIOJ
@ -525,6 +536,10 @@
#define AUDIO_DMA DMA1 #define AUDIO_DMA DMA1
#endif #endif
#if defined(RADIO_T16)
#define AUDIO_UNMUTE_DELAY 150
#endif
// I2C Bus // I2C Bus
#define I2C_RCC_AHB1Periph RCC_AHB1Periph_GPIOB #define I2C_RCC_AHB1Periph RCC_AHB1Periph_GPIOB
#define I2C_RCC_APB1Periph RCC_APB1Periph_I2C1 #define I2C_RCC_APB1Periph RCC_APB1Periph_I2C1
@ -563,9 +578,9 @@
#define HAPTIC_TIMER_COMPARE_VALUE HAPTIC_GPIO_TIMER->CCR2 #define HAPTIC_TIMER_COMPARE_VALUE HAPTIC_GPIO_TIMER->CCR2
#endif #endif
// Internal Module #if !defined(RADIO_T16)
#define HARDWARE_INTERNAL_MODULE
#define EXTERNAL_ANTENNA #define EXTERNAL_ANTENNA
#endif
#define INTMODULE_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA2) #define INTMODULE_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA2)
#define INTMODULE_PWR_GPIO GPIOA #define INTMODULE_PWR_GPIO GPIOA
#define INTMODULE_PWR_GPIO_PIN GPIO_Pin_8 // PA.08 #define INTMODULE_PWR_GPIO_PIN GPIO_Pin_8 // PA.08

View file

@ -254,7 +254,7 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
} }
// TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp // TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp
uint8_t telemetryGetByte(uint8_t * byte) bool telemetryGetByte(uint8_t * byte)
{ {
#if defined(PCBX12S) #if defined(PCBX12S)
if (telemetryFifoMode & TELEMETRY_SERIAL_WITHOUT_DMA) if (telemetryFifoMode & TELEMETRY_SERIAL_WITHOUT_DMA)

View file

@ -10,6 +10,7 @@ set(TARGET_DIR sky9x)
set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} 9x_bitmaps) set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} 9x_bitmaps)
set(PPM_LIMITS_SYMETRICAL YES) set(PPM_LIMITS_SYMETRICAL YES)
set(PXX1 YES) set(PXX1 YES)
add_definitions(-DDISABLE_MULTI_UPDATE)
if(PCB STREQUAL 9XRPRO) if(PCB STREQUAL 9XRPRO)
add_definitions(-Dat91sam3s8 -DREVX) add_definitions(-Dat91sam3s8 -DREVX)

View file

@ -1,12 +1,13 @@
option(SHUTDOWN_CONFIRMATION "Shutdown confirmation" OFF) option(SHUTDOWN_CONFIRMATION "Shutdown confirmation" OFF)
option(LCD_DUAL_BUFFER "Dual LCD Buffer" OFF) option(LCD_DUAL_BUFFER "Dual LCD Buffer" OFF)
option(INTERNAL_MODULE_PPM "Support for PPM internal module hack" OFF)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module replacement" OFF)
option(PXX1 "PXX1 protocol support" ON) option(PXX1 "PXX1 protocol support" ON)
option(PXX2 "PXX2 protocol support" OFF) option(PXX2 "PXX2 protocol support" OFF)
option(INTERNAL_MODULE_PPM "Support for PPM internal module" OFF)
option(AUTOUPDATE "Auto update internal chips from SD" OFF) option(AUTOUPDATE "Auto update internal chips from SD" OFF)
if(PCB STREQUAL X9E) if(PCB STREQUAL X9E)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(STICKS "STANDARD" CACHE STRING "Sticks type (STANDARD/HORUS)") set(STICKS "STANDARD" CACHE STRING "Sticks type (STANDARD/HORUS)")
set(CPU_TYPE STM32F4) set(CPU_TYPE STM32F4)
@ -49,6 +50,8 @@ elseif(PCB STREQUAL X9D+)
set(PCBREV 2014 CACHE STRING "PCB Revision") set(PCBREV 2014 CACHE STRING "PCB Revision")
add_definitions(-DPCBREV=${PCBREV}) add_definitions(-DPCBREV=${PCBREV})
if (${PCBREV} STREQUAL 2019) if (${PCBREV} STREQUAL 2019)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON)
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(CPU_TYPE STM32F4) set(CPU_TYPE STM32F4)
set(CPU_TYPE_FULL STM32F407xE) # for size report set(CPU_TYPE_FULL STM32F407xE) # for size report
@ -64,6 +67,8 @@ elseif(PCB STREQUAL X9D+)
set(PXX_FREQUENCY "HIGH") set(PXX_FREQUENCY "HIGH")
set(FLAVOUR x9d+2019) set(FLAVOUR x9d+2019)
else() else()
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
set(CPU_TYPE_FULL STM32F205xE) # for size report set(CPU_TYPE_FULL STM32F205xE) # for size report
set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld) set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld)
@ -72,6 +77,8 @@ elseif(PCB STREQUAL X9D+)
set(FLAVOUR x9d+) set(FLAVOUR x9d+)
endif() endif()
elseif(PCB STREQUAL X9D) elseif(PCB STREQUAL X9D)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
set(CPU_TYPE_FULL STM32F205xE) # for size report set(CPU_TYPE_FULL STM32F205xE) # for size report
set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld) set(LINKER_SCRIPT targets/taranis/stm32f2_flash.ld)
@ -101,6 +108,8 @@ elseif(PCB STREQUAL X7)
add_definitions(-DRADIO_T12) add_definitions(-DRADIO_T12)
add_definitions(-DEEPROM_VARIANT=0x4001) add_definitions(-DEEPROM_VARIANT=0x4001)
else() else()
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(LUA_EXPORT lua_export_x7) set(LUA_EXPORT lua_export_x7)
set(FLAVOUR x7) set(FLAVOUR x7)
set(NAVIGATION_TYPE x7) set(NAVIGATION_TYPE x7)
@ -116,6 +125,8 @@ elseif(PCB STREQUAL X7)
set(GVAR_SCREEN model_gvars.cpp) set(GVAR_SCREEN model_gvars.cpp)
set(STATUS_LEDS YES) set(STATUS_LEDS YES)
elseif(PCB STREQUAL X9LITE) elseif(PCB STREQUAL X9LITE)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON)
set(PXX_FREQUENCY "HIGH") set(PXX_FREQUENCY "HIGH")
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
@ -137,6 +148,8 @@ elseif(PCB STREQUAL X9LITE)
set(GVAR_SCREEN model_gvars.cpp) set(GVAR_SCREEN model_gvars.cpp)
set(STATUS_LEDS YES) set(STATUS_LEDS YES)
elseif(PCB STREQUAL X9LITES) elseif(PCB STREQUAL X9LITES)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON)
set(PXX_FREQUENCY "HIGH") set(PXX_FREQUENCY "HIGH")
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
@ -158,6 +171,8 @@ elseif(PCB STREQUAL X9LITES)
set(GVAR_SCREEN model_gvars.cpp) set(GVAR_SCREEN model_gvars.cpp)
set(STATUS_LEDS YES) set(STATUS_LEDS YES)
elseif(PCB STREQUAL XLITE) elseif(PCB STREQUAL XLITE)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" OFF)
set(PXX_FREQUENCY "HIGH" CACHE STRING "PXX frequency (LOW / HIGH)") # always use HIGH except on some prototype boards set(PXX_FREQUENCY "HIGH" CACHE STRING "PXX frequency (LOW / HIGH)") # always use HIGH except on some prototype boards
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
@ -178,6 +193,8 @@ elseif(PCB STREQUAL XLITE)
set(GVAR_SCREEN model_gvars.cpp) set(GVAR_SCREEN model_gvars.cpp)
set(STATUS_LEDS YES) set(STATUS_LEDS YES)
elseif(PCB STREQUAL XLITES) elseif(PCB STREQUAL XLITES)
option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" OFF)
option(INTERNAL_MODULE_PXX2 "Support for PXX2 internal module" ON)
set(PXX_FREQUENCY "HIGH") set(PXX_FREQUENCY "HIGH")
set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)") set(PWR_BUTTON "PRESS" CACHE STRING "Pwr button type (PRESS/SWITCH)")
set(CPU_TYPE STM32F2) set(CPU_TYPE STM32F2)
@ -266,6 +283,7 @@ add_definitions(-DAUDIO -DVOICE -DRTCLOCK)
set(SRC set(SRC
${SRC} ${SRC}
io/frsky_firmware_update.cpp io/frsky_firmware_update.cpp
io/multi_firmware_update.cpp
) )
set(GUI_SRC set(GUI_SRC
@ -322,12 +340,20 @@ if(LCD_DUAL_BUFFER)
add_definitions(-DLCD_DUAL_BUFFER) add_definitions(-DLCD_DUAL_BUFFER)
endif() endif()
if(INTERNAL_MODULE_PPM)
add_definitions(-DINTERNAL_MODULE_PPM)
endif()
if(INTERNAL_MODULE_PXX1) if(INTERNAL_MODULE_PXX1)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_PXX1) add_definitions(-DINTERNAL_MODULE_PXX1)
endif() endif()
if(INTERNAL_MODULE_PXX2)
set(PXX2 ON)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_PXX2)
endif()
if(INTERNAL_MODULE_PPM)
add_definitions(-DHARDWARE_INTERNAL_MODULE)
add_definitions(-DINTERNAL_MODULE_PPM)
endif()
set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} ${BITMAPS_TARGET}) set(FIRMWARE_DEPENDENCIES ${FIRMWARE_DEPENDENCIES} ${BITMAPS_TARGET})

View file

@ -121,7 +121,7 @@ uint32_t isBootloaderStart(const uint8_t * buffer);
#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET)
#endif #endif
void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable); void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable, uint16_t parity, uint16_t stopBits, uint16_t wordLength);
void intmoduleSendByte(uint8_t byte); void intmoduleSendByte(uint8_t byte);
void intmoduleSendBuffer(const uint8_t * data, uint8_t size); void intmoduleSendBuffer(const uint8_t * data, uint8_t size);
void intmoduleSendNextFrame(); void intmoduleSendNextFrame();
@ -130,6 +130,7 @@ void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inver
void extmoduleInvertedSerialStart(uint32_t baudrate); void extmoduleInvertedSerialStart(uint32_t baudrate);
void extmoduleSendBuffer(const uint8_t * data, uint8_t size); void extmoduleSendBuffer(const uint8_t * data, uint8_t size);
void extmoduleSendNextFrame(); void extmoduleSendNextFrame();
void extmoduleSendInvertedByte(uint8_t byte);
// Trainer driver // Trainer driver
#define SLAVE_MODE() (g_model.trainerData.mode == TRAINER_MODE_SLAVE) #define SLAVE_MODE() (g_model.trainerData.mode == TRAINER_MODE_SLAVE)
@ -670,7 +671,7 @@ void sportSendByte(uint8_t byte);
void sportSendByteLoop(uint8_t byte); void sportSendByteLoop(uint8_t byte);
void sportStopSendByteLoop(); void sportStopSendByteLoop();
void sportSendBuffer(const uint8_t * buffer, uint32_t count); void sportSendBuffer(const uint8_t * buffer, uint32_t count);
uint8_t telemetryGetByte(uint8_t * byte); bool telemetryGetByte(uint8_t * byte);
void telemetryClearFifo(); void telemetryClearFifo();
extern uint32_t telemetryErrors; extern uint32_t telemetryErrors;

View file

@ -309,6 +309,38 @@ void extmoduleSendNextFrame()
} }
} }
void extmoduleSendInvertedByte(uint8_t byte)
{
uint16_t time;
uint32_t i;
__disable_irq();
time = getTmr2MHz();
GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
while ((uint16_t) (getTmr2MHz() - time) < 34) {
// wait
}
time += 34;
for (i = 0 ; i < 8 ; i += 1) {
if (byte & 1) {
GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
}
else {
GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
}
byte >>= 1 ;
while ((uint16_t) (getTmr2MHz() - time) < 35) {
// wait
}
time += 35 ;
}
GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
__enable_irq() ; // No need to wait for the stop bit to complete
while ((uint16_t) (getTmr2MHz() - time) < 34) {
// wait
}
}
extern "C" void EXTMODULE_TIMER_DMA_STREAM_IRQHandler() extern "C" void EXTMODULE_TIMER_DMA_STREAM_IRQHandler()
{ {
if (!DMA_GetITStatus(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC)) if (!DMA_GetITStatus(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC))

View file

@ -929,17 +929,9 @@
#endif #endif
// Internal Module // Internal Module
#if !defined(RADIO_T12)
#define HARDWARE_INTERNAL_MODULE
#endif
#if defined(PCBXLITE) #if defined(PCBXLITE)
#define EXTERNAL_ANTENNA #define EXTERNAL_ANTENNA
#endif #endif
#if defined(PCBXLITES) || defined(PCBX9LITE) || (defined(PCBX9DP) && PCBREV >= 2019)
#define INTERNAL_MODULE_PXX2
#else
#define INTERNAL_MODULE_PXX1
#endif
#if defined(PCBXLITE) || defined(PCBX9LITE) #if defined(PCBXLITE) || defined(PCBX9LITE)
#define INTMODULE_RCC_APB1Periph 0 #define INTMODULE_RCC_APB1Periph 0
#define INTMODULE_RCC_APB2Periph RCC_APB2Periph_USART1 #define INTMODULE_RCC_APB2Periph RCC_APB2Periph_USART1

View file

@ -218,7 +218,7 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
} }
// TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp // TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp
uint8_t telemetryGetByte(uint8_t * byte) bool telemetryGetByte(uint8_t * byte)
{ {
#if defined(AUX_SERIAL) #if defined(AUX_SERIAL)
if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) { if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {

View file

@ -133,35 +133,35 @@ void processFlySkyPacket(const uint8_t *packet)
} }
} }
void processFlySkyTelemetryData(uint8_t data) void processFlySkyTelemetryData(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount)
{ {
if (telemetryRxBufferCount == 0 && data != 0xAA) { if (rxBufferCount == 0 && data != 0xAA) {
TRACE("[IBUS] invalid start byte 0x%02X", data); TRACE("[IBUS] invalid start byte 0x%02X", data);
return; return;
} }
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) { if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data; rxBuffer[rxBufferCount++] = data;
} }
else { else {
TRACE("[IBUS] array size %d error", telemetryRxBufferCount); TRACE("[IBUS] array size %d error", rxBufferCount);
telemetryRxBufferCount = 0; rxBufferCount = 0;
} }
if (telemetryRxBufferCount >= FLYSKY_TELEMETRY_LENGTH) { if (rxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
// debug print the content of the packets // debug print the content of the packets
#if 0 #if 0
debugPrintf("[IBUS] Packet 0x%02X rssi 0x%02X: ", debugPrintf("[IBUS] Packet 0x%02X rssi 0x%02X: ",
telemetryRxBuffer[0], telemetryRxBuffer[1]); rxBuffer[0], rxBuffer[1]);
for (int i=0; i<7; i++) { for (int i=0; i<7; i++) {
debugPrintf("[%02X %02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3], debugPrintf("[%02X %02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]); rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
} }
debugPrintf("\r\n"); debugPrintf("\r\n");
#endif #endif
processFlySkyPacket(telemetryRxBuffer+1); processFlySkyPacket(rxBuffer+1);
telemetryRxBufferCount = 0; rxBufferCount = 0;
} }
} }

View file

@ -21,7 +21,7 @@
#ifndef _FLYSKY_IBUS_H #ifndef _FLYSKY_IBUS_H
#define _FLYSKY_IBUS_H #define _FLYSKY_IBUS_H
void processFlySkyTelemetryData(uint8_t data); void processFlySkyTelemetryData(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount);
void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance); void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
// Used by multi protocol // Used by multi protocol

View file

@ -21,11 +21,6 @@
#include "telemetry.h" #include "telemetry.h"
#include "multi.h" #include "multi.h"
MultiModuleStatus multiModuleStatus;
MultiModuleSyncStatus multiSyncStatus;
uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
enum MultiPacketTypes : uint8_t { enum MultiPacketTypes : uint8_t {
MultiStatus = 1, MultiStatus = 1,
FrSkySportTelemtry, FrSkySportTelemtry,
@ -51,53 +46,142 @@ enum MultiBufferState : uint8_t {
MultiStatusOrFrskyData MultiStatusOrFrskyData
}; };
MultiBufferState guessProtocol()
#if defined(INTERNAL_MODULE_MULTI)
static MultiModuleStatus multiModuleStatus[NUM_MODULES] = { MultiModuleStatus(), MultiModuleStatus() };
static MultiModuleSyncStatus multiSyncStatus[NUM_MODULES] = { MultiModuleSyncStatus(), MultiModuleSyncStatus() };
static uint8_t multiBindStatus[NUM_MODULES] = { MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION };
static MultiBufferState multiTelemetryBufferState[NUM_MODULES];
MultiModuleStatus& getMultiModuleStatus(uint8_t module)
{ {
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2) return multiModuleStatus[module];
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module)
{
return multiSyncStatus[module];
}
uint8_t getMultiBindStatus(uint8_t module)
{
return multiBindStatus[module];
}
void setMultiBindStatus(uint8_t module, uint8_t bindStatus)
{
multiBindStatus[module] = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t module)
{
return multiTelemetryBufferState[module];
}
void setMultiTelemetryBufferState(uint8_t module, MultiBufferState state)
{
multiTelemetryBufferState[module] = state;
}
// Use additional telemetry buffer
uint8_t intTelemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
uint8_t intTelemetryRxBufferCount;
#else // !INTERNAL_MODULE_MULTI
static MultiModuleStatus multiModuleStatus;
static MultiModuleSyncStatus multiSyncStatus;
static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
static MultiBufferState multiTelemetryBufferState;
MultiModuleStatus& getMultiModuleStatus(uint8_t)
{
return multiModuleStatus;
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
{
return multiSyncStatus;
}
uint8_t getMultiBindStatus(uint8_t)
{
return multiBindStatus;
}
void setMultiBindStatus(uint8_t, uint8_t bindStatus)
{
multiBindStatus = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t)
{
return multiTelemetryBufferState;
}
void setMultiTelemetryBufferState(uint8_t, MultiBufferState state)
{
multiTelemetryBufferState = state;
}
#endif // INTERNAL_MODULE_MULTI
static MultiBufferState guessProtocol(uint8_t module)
{
if (g_model.moduleData[module].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2)
return SpektrumTelemetryFallback; return SpektrumTelemetryFallback;
else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A) else if (g_model.moduleData[module].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
return FlyskyTelemetryFallback; return FlyskyTelemetryFallback;
else else
return FrskyTelemetryFallback; return FrskyTelemetryFallback;
} }
static void processMultiStatusPacket(const uint8_t *data) static void processMultiStatusPacket(const uint8_t *data, uint8_t module)
{ {
MultiModuleStatus& status = getMultiModuleStatus(module);
// At least two status packets without bind flag // At least two status packets without bind flag
bool wasBinding = (multiModuleStatus.isBinding()); bool wasBinding = status.isBinding();
multiModuleStatus.flags = data[0]; status.flags = data[0];
multiModuleStatus.major = data[1]; status.major = data[1];
multiModuleStatus.minor = data[2]; status.minor = data[2];
multiModuleStatus.revision = data[3]; status.revision = data[3];
multiModuleStatus.patch = data[4]; status.patch = data[4];
multiModuleStatus.lastUpdate = get_tmr10ms(); status.lastUpdate = get_tmr10ms();
if (wasBinding && !multiModuleStatus.isBinding() && multiBindStatus == MULTI_BIND_INITIATED) if (wasBinding && !status.isBinding() && getMultiBindStatus(module) == MULTI_BIND_INITIATED)
multiBindStatus = MULTI_BIND_FINISHED; setMultiBindStatus(module, MULTI_BIND_FINISHED);
} }
static void processMultiSyncPacket(const uint8_t *data) static void processMultiSyncPacket(const uint8_t *data, uint8_t module)
{ {
multiSyncStatus.lastUpdate = get_tmr10ms(); MultiModuleSyncStatus& status = getMultiSyncStatus(module);
multiSyncStatus.interval = data[4];
multiSyncStatus.target = data[5]; status.lastUpdate = get_tmr10ms();
status.interval = data[4];
status.target = data[5];
#if !defined(PPM_PIN_SERIAL) #if !defined(PPM_PIN_SERIAL)
auto oldlag = multiSyncStatus.inputLag; auto oldlag = status.inputLag;
(void) oldlag; (void) oldlag;
#endif #endif
multiSyncStatus.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]); status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
#if !defined(PPM_PIN_SERIAL) #if !defined(PPM_PIN_SERIAL)
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n", extmodulePulsesData.dsm2.rest, TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
multiSyncStatus.inputLag, oldlag-multiSyncStatus.inputLag, multiSyncStatus.target, multiSyncStatus.interval, multiSyncStatus.refreshRate, multiSyncStatus.adjustedRefreshRate/50, module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
multiSyncStatus.getAdjustedRefreshRate()); status.inputLag, oldlag-status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate/50,
status.getAdjustedRefreshRate());
#endif #endif
} }
static void processMultiTelemetryPaket(const uint8_t *packet) static void processMultiTelemetryPaket(const uint8_t *packet, uint8_t module)
{ {
uint8_t type = packet[0]; uint8_t type = packet[0];
uint8_t len = packet[1]; uint8_t len = packet[1];
@ -107,12 +191,12 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
switch (type) { switch (type) {
case MultiStatus: case MultiStatus:
if (len >= 5) if (len >= 5)
processMultiStatusPacket(data); processMultiStatusPacket(data, module);
break; break;
case DSMBindPacket: case DSMBindPacket:
if (len >= 10) if (len >= 10)
processDSMBindPacket(data); processDSMBindPacket(module, data);
break; break;
case SpektrumTelemetry: case SpektrumTelemetry:
@ -147,7 +231,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
case InputSync: case InputSync:
if (len >= 6) if (len >= 6)
processMultiSyncPacket(data); processMultiSyncPacket(data, module);
else else
TRACE("[MP] Received input sync len %d < 6", len); TRACE("[MP] Received input sync len %d < 6", len);
break; break;
@ -294,9 +378,11 @@ void MultiModuleStatus::getStatusString(char *statusText)
{ {
if (!isValid()) { if (!isValid()) {
#if defined(PCBTARANIS) || defined(PCBHORUS) #if defined(PCBTARANIS) || defined(PCBHORUS)
#if !defined(INTERNAL_MODULE_MULTI)
if (IS_INTERNAL_MODULE_ENABLED()) if (IS_INTERNAL_MODULE_ENABLED())
strcpy(statusText, STR_DISABLE_INTERNAL); strcpy(statusText, STR_DISABLE_INTERNAL);
else else
#endif
#endif #endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY); strcpy(statusText, STR_MODULE_NO_TELEMETRY);
return; return;
@ -333,50 +419,71 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcat(statusText, STR_MODULE_BINDING); strcat(statusText, STR_MODULE_BINDING);
} }
static uint8_t* getRxBuffer(uint8_t moduleIdx)
static MultiBufferState multiTelemetryBufferState;
static void processMultiTelemetryByte(const uint8_t data)
{ {
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) { #if defined(INTERNAL_MODULE_MULTI)
telemetryRxBuffer[telemetryRxBufferCount++] = data; if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBuffer;
#endif
return telemetryRxBuffer;
}
static uint8_t& getRxBufferCount(uint8_t moduleIdx)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBufferCount;
#endif
return telemetryRxBufferCount;
}
static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
{
uint8_t* rxBuffer = getRxBuffer(module);
uint8_t& rxBufferCount = getRxBufferCount(module);
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
} }
else { else {
TRACE("[MP] array size %d error", telemetryRxBufferCount); TRACE("[MP] array size %d error", rxBufferCount);
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
} }
// Length field does not count the header // Length field does not count the header
if (telemetryRxBufferCount >= 2 && telemetryRxBuffer[1] == telemetryRxBufferCount - 2) { if (rxBufferCount >= 2 && rxBuffer[1] == rxBufferCount - 2) {
// debug print the content of the packet // debug print the content of the packet
#if 0 #if 0
debugPrintf("[MP] Packet type %02X len 0x%02X: ", debugPrintf("[MP] Packet type %02X len 0x%02X: ",
telemetryRxBuffer[0], telemetryRxBuffer[1]); rxBuffer[0], rxBuffer[1]);
for (int i=0; i<(telemetryRxBufferCount+3)/4; i++) { for (int i=0; i<(rxBufferCount+3)/4; i++) {
debugPrintf("[%02X%02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3], debugPrintf("[%02X%02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]); rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
} }
debugPrintf("\r\n"); debugPrintf("\r\n");
#endif #endif
// Packet is complete, process it // Packet is complete, process it
processMultiTelemetryPaket(telemetryRxBuffer); processMultiTelemetryPaket(rxBuffer, module);
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
} }
} }
void processMultiTelemetryData(const uint8_t data) void processMultiTelemetryData(uint8_t data, uint8_t module)
{ {
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, telemetryRxBufferCount); uint8_t* rxBuffer = getRxBuffer(module);
switch (multiTelemetryBufferState) { uint8_t& rxBufferCount = getRxBufferCount(module);
debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, rxBufferCount);
switch (getMultiTelemetryBufferState(module)) {
case NoProtocolDetected: case NoProtocolDetected:
if (data == 'M') { if (data == 'M') {
multiTelemetryBufferState = MultiFirstByteReceived; setMultiTelemetryBufferState(module, MultiFirstByteReceived);
} }
else if (data == 0xAA || data == 0x7e) { else if (data == 0xAA || data == 0x7e) {
multiTelemetryBufferState = guessProtocol(); setMultiTelemetryBufferState(module, guessProtocol(module));
// Process the first byte by the protocol // Process the first byte by the protocol
processMultiTelemetryData(data); processMultiTelemetryData(data, module);
} }
else { else {
TRACE("[MP] invalid start byte 0x%02X", data); TRACE("[MP] invalid start byte 0x%02X", data);
@ -384,91 +491,92 @@ void processMultiTelemetryData(const uint8_t data)
break; break;
case FrskyTelemetryFallback: case FrskyTelemetryFallback:
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte; setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
processFrskyTelemetryData(data); processFrskyTelemetryData(data);
break; break;
case FrskyTelemetryFallbackFirstByte: case FrskyTelemetryFallbackFirstByte:
if (data == 'M') { if (data == 'M') {
multiTelemetryBufferState = MultiStatusOrFrskyData; setMultiTelemetryBufferState(module, MultiStatusOrFrskyData);
} }
else { else {
processFrskyTelemetryData(data); processFrskyTelemetryData(data);
if (data != 0x7e) if (data != 0x7e)
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes; setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
} }
break; break;
case FrskyTelemetryFallbackNextBytes: case FrskyTelemetryFallbackNextBytes:
processFrskyTelemetryData(data); processFrskyTelemetryData(data);
if (data == 0x7e) if (data == 0x7e) {
// end of packet or start of new packet // end of packet or start of new packet
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte; setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
}
break; break;
case FlyskyTelemetryFallback: case FlyskyTelemetryFallback:
processFlySkyTelemetryData(data); processFlySkyTelemetryData(data, rxBuffer, rxBufferCount);
if (telemetryRxBufferCount == 0) if (rxBufferCount == 0) {
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break; break;
case SpektrumTelemetryFallback: case SpektrumTelemetryFallback:
processSpektrumTelemetryData(data); processSpektrumTelemetryData(module, data,rxBuffer, rxBufferCount);
if (telemetryRxBufferCount == 0) if (rxBufferCount == 0) {
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break; break;
case MultiFirstByteReceived: case MultiFirstByteReceived:
telemetryRxBufferCount = 0; rxBufferCount = 0;
if (data == 'P') { if (data == 'P') {
multiTelemetryBufferState = ReceivingMultiProtocol; setMultiTelemetryBufferState(module, ReceivingMultiProtocol);
} }
else if (data >= 5 && data <= 10) { else if (data >= 5 && data <= 10) {
// Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have // Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
// a bit of validation // a bit of validation
multiTelemetryBufferState = ReceivingMultiStatus; setMultiTelemetryBufferState(module, ReceivingMultiStatus);
processMultiTelemetryData(data); processMultiTelemetryData(data, module);
} }
else { else {
TRACE("[MP] invalid second byte 0x%02X", data); TRACE("[MP] invalid second byte 0x%02X", data);
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
} }
break; break;
case ReceivingMultiProtocol: case ReceivingMultiProtocol:
processMultiTelemetryByte(data); processMultiTelemetryByte(data, module);
break; break;
case MultiStatusOrFrskyData: case MultiStatusOrFrskyData:
// Check len byte if it makes sense for multi // Check len byte if it makes sense for multi
if (data >= 5 && data <= 10) { if (data >= 5 && data <= 10) {
multiTelemetryBufferState = ReceivingMultiStatus; setMultiTelemetryBufferState(module, ReceivingMultiStatus);
telemetryRxBufferCount = 0; rxBufferCount = 0;
} }
else { else {
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes; setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
processMultiTelemetryData('M'); processMultiTelemetryData('M', module);
} }
processMultiTelemetryData(data); processMultiTelemetryData(data, module);
break; break;
case ReceivingMultiStatus: case ReceivingMultiStatus:
telemetryRxBuffer[telemetryRxBufferCount++] = data; rxBuffer[rxBufferCount++] = data;
if (telemetryRxBufferCount > 5 && telemetryRxBuffer[0] == telemetryRxBufferCount-1) { if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount-1) {
processMultiStatusPacket(telemetryRxBuffer+1); processMultiStatusPacket(rxBuffer+1, module);
telemetryRxBufferCount = 0; rxBufferCount = 0;
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
} }
if (telemetryRxBufferCount > 10) { if (rxBufferCount > 10) {
// too long ignore // too long ignore
TRACE("Overlong multi status packet detected ignoring, wanted %d", telemetryRxBuffer[0]); TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]);
telemetryRxBufferCount =0; rxBufferCount =0;
multiTelemetryBufferState = NoProtocolDetected; setMultiTelemetryBufferState(module, NoProtocolDetected);
} }
break;
} }
} }

View file

@ -82,9 +82,7 @@ Type 0x06 Flysky AFHDS2 telemetry data
*/ */
void processMultiTelemetryData(uint8_t data, uint8_t module);
void processMultiTelemetryData(uint8_t data);
// This should be put into the Module definition if other modules gain this functionality // This should be put into the Module definition if other modules gain this functionality
struct MultiModuleSyncStatus { struct MultiModuleSyncStatus {
@ -107,7 +105,7 @@ struct MultiModuleSyncStatus {
}; };
extern MultiModuleSyncStatus multiSyncStatus; MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module);
struct MultiModuleStatus { struct MultiModuleStatus {
@ -131,13 +129,15 @@ struct MultiModuleStatus {
inline bool inputDetected() { return (bool) (flags & 0x01); } inline bool inputDetected() { return (bool) (flags & 0x01); }
}; };
extern MultiModuleStatus multiModuleStatus; MultiModuleStatus& getMultiModuleStatus(uint8_t module);
enum MultiBindStatus : uint8_t { enum MultiBindStatus : uint8_t {
MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION,
MULTI_BIND_INITIATED, MULTI_BIND_INITIATED,
MULTI_BIND_FINISHED, MULTI_BIND_FINISHED,
}; };
extern uint8_t multiBindStatus; uint8_t getMultiBindStatus(uint8_t module);
void setMultiBindStatus(uint8_t module, uint8_t bindStatus);
#endif //OPENTX_MULTI_H #endif //OPENTX_MULTI_H

View file

@ -370,28 +370,27 @@ void processSpektrumPacket(const uint8_t *packet)
LemonRX+Sat+tele 0xb2 07 1 LemonRX+Sat+tele 0xb2 07 1
*/ */
void processDSMBindPacket(const uint8_t *packet) void processDSMBindPacket(uint8_t module, const uint8_t *packet)
{ {
uint32_t debugval; uint32_t debugval;
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2 if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[module].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2
&& g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode) { && g_model.moduleData[module].multi.autoBindMode) {
int channels = packet[5]; int channels = packet[5];
// Only sets channel etc when in DSM multi mode // Only sets channel etc when in DSM multi mode
g_model.moduleData[EXTERNAL_MODULE].channelsCount = channels - 8; g_model.moduleData[module].channelsCount = channels - 8;
// bool use11ms = (packet[8] & 0x10) ; // bool use11ms = (packet[8] & 0x10) ;
if (packet[6] >= 0xb2) if (packet[6] >= 0xb2)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_11; g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
else if (packet[6] >= 0xa2) else if (packet[6] >= 0xa2)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_22; g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
else if (packet[6] >= 0x12) else if (packet[6] >= 0x12)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_11; g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
else else
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_22; g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
storageDirty(EE_MODEL); storageDirty(EE_MODEL);
} }
debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4]; debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4];
@ -400,45 +399,45 @@ void processDSMBindPacket(const uint8_t *packet)
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 4, 0, 0, debugval, UNIT_RAW, 0); setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 4, 0, 0, debugval, UNIT_RAW, 0);
/* Finally stop binding as the rx just told us that it is bound */ /* Finally stop binding as the rx just told us that it is bound */
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2 && moduleState[EXTERNAL_MODULE].mode == MODULE_MODE_BIND) { if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[module].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2 && moduleState[module].mode == MODULE_MODE_BIND) {
multiBindStatus=MULTI_BIND_FINISHED; setMultiBindStatus(module, MULTI_BIND_FINISHED);
} }
} }
void processSpektrumTelemetryData(uint8_t data) void processSpektrumTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount)
{ {
if (telemetryRxBufferCount == 0 && data != 0xAA) { if (rxBufferCount == 0 && data != 0xAA) {
TRACE("[SPK] invalid start byte 0x%02X", data); TRACE("[SPK] invalid start byte 0x%02X", data);
return; return;
} }
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) { if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data; rxBuffer[rxBufferCount++] = data;
} }
else { else {
TRACE("[SPK] array size %d error", telemetryRxBufferCount); TRACE("[SPK] array size %d error", rxBufferCount);
telemetryRxBufferCount = 0; rxBufferCount = 0;
} }
if (telemetryRxBuffer[1] == 0x80 && telemetryRxBufferCount >= DSM_BIND_PACKET_LENGTH) { if (rxBuffer[1] == 0x80 && rxBufferCount >= DSM_BIND_PACKET_LENGTH) {
processDSMBindPacket(telemetryRxBuffer+2); processDSMBindPacket(module, rxBuffer+2);
telemetryRxBufferCount = 0; rxBufferCount = 0;
return; return;
} }
if (telemetryRxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) { if (rxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) {
// Debug print content of Telemetry to console // Debug print content of Telemetry to console
#if 0 #if 0
debugPrintf("[SPK] Packet 0x%02X rssi 0x%02X: ic2 0x%02x, %02x: ", debugPrintf("[SPK] Packet 0x%02X rssi 0x%02X: ic2 0x%02x, %02x: ",
telemetryRxBuffer[0], telemetryRxBuffer[1], telemetryRxBuffer[2], telemetryRxBuffer[3]); rxBuffer[0], rxBuffer[1], rxBuffer[2], rxBuffer[3]);
for (int i=4; i<SPEKTRUM_TELEMETRY_LENGTH; i+=4) { for (int i=4; i<SPEKTRUM_TELEMETRY_LENGTH; i+=4) {
debugPrintf("%02X%02X %02X%02X ", telemetryRxBuffer[i], telemetryRxBuffer[i + 1], debugPrintf("%02X%02X %02X%02X ", rxBuffer[i], rxBuffer[i + 1],
telemetryRxBuffer[i + 2], telemetryRxBuffer[i + 3]); rxBuffer[i + 2], rxBuffer[i + 3]);
} }
debugPrintf("\r\n"); debugPrintf("\r\n");
#endif #endif
processSpektrumPacket(telemetryRxBuffer); processSpektrumPacket(rxBuffer);
telemetryRxBufferCount = 0; rxBufferCount = 0;
} }
} }

View file

@ -21,10 +21,10 @@
#ifndef _SPEKTRUM_H #ifndef _SPEKTRUM_H
#define _SPEKTRUM_H #define _SPEKTRUM_H
void processSpektrumTelemetryData(uint8_t data); void processSpektrumTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount);
void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance); void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
// Used directly by multi telemetry protocol // Used directly by multi telemetry protocol
void processSpektrumPacket(const uint8_t *packet); void processSpektrumPacket(const uint8_t *packet);
void processDSMBindPacket(const uint8_t *packet); void processDSMBindPacket(uint8_t module, const uint8_t *packet);
#endif #endif

View file

@ -19,6 +19,7 @@
*/ */
#include "opentx.h" #include "opentx.h"
#include "multi.h"
uint8_t telemetryStreaming = 0; uint8_t telemetryStreaming = 0;
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
@ -45,15 +46,15 @@ void processTelemetryData(uint8_t data)
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM) { if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
processSpektrumTelemetryData(data); processSpektrumTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer, telemetryRxBufferCount);
return; return;
} }
if (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) { if (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
processFlySkyTelemetryData(data); processFlySkyTelemetryData(data, telemetryRxBuffer, telemetryRxBufferCount);
return; return;
} }
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) { if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
processMultiTelemetryData(data); processMultiTelemetryData(data, EXTERNAL_MODULE);
return; return;
} }
#endif #endif
@ -91,22 +92,29 @@ void telemetryWakeup()
} }
#endif #endif
#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) #if defined(INTERNAL_MODULE_PXX2) || defined(EXTMODULE_USART)
uint8_t frame[PXX2_FRAME_MAXLENGTH]; uint8_t frame[PXX2_FRAME_MAXLENGTH];
#if defined(INTMODULE_USART) #if defined(INTERNAL_MODULE_PXX2)
while (intmoduleFifo.getFrame(frame)) { while (intmoduleFifo.getFrame(frame)) {
processPXX2Frame(INTERNAL_MODULE, frame); processPXX2Frame(INTERNAL_MODULE, frame);
} }
#endif #endif
#if defined(EXTMODULE_USART) #if defined(EXTMODULE_USART)
while (extmoduleFifo.getFrame(frame)) { while (extmoduleFifo.getFrame(frame)) {
processPXX2Frame(EXTERNAL_MODULE, frame); processPXX2Frame(EXTERNAL_MODULE, frame);
} }
#endif #endif
#endif #endif
#if defined(INTERNAL_MODULE_MULTI)
while(!intmoduleFifo.isEmpty()) {
uint8_t b=0;
intmoduleFifo.pop(b);
processMultiTelemetryData(b, INTERNAL_MODULE);
}
#endif
#if defined(STM32) #if defined(STM32)
uint8_t data; uint8_t data;
if (telemetryGetByte(&data)) { if (telemetryGetByte(&data)) {

View file

@ -112,22 +112,27 @@ extern uint8_t telemetryProtocol;
inline uint8_t modelTelemetryProtocol() inline uint8_t modelTelemetryProtocol()
{ {
bool internalModuleInUse = IS_INTERNAL_MODULE_ENABLED();
#if defined(INTERNAL_MODULE_MULTI)
//internal muli module is not conflicting with external one
internalModuleInUse = false;
#endif
#if defined(CROSSFIRE) #if defined(CROSSFIRE)
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_CROSSFIRE) { if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_CROSSFIRE) {
return PROTOCOL_TELEMETRY_CROSSFIRE; return PROTOCOL_TELEMETRY_CROSSFIRE;
} }
#endif #endif
if (!IS_INTERNAL_MODULE_ENABLED() && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) { if (!internalModuleInUse && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) {
return g_model.telemetryProtocol; return g_model.telemetryProtocol;
} }
#if defined(MULTIMODULE) #if defined(MULTIMODULE)
if (!IS_INTERNAL_MODULE_ENABLED() && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) { if (!internalModuleInUse && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) {
return PROTOCOL_TELEMETRY_MULTIMODULE; return PROTOCOL_TELEMETRY_MULTIMODULE;
} }
#endif #endif
// default choice // default choice
return PROTOCOL_TELEMETRY_FRSKY_SPORT; return PROTOCOL_TELEMETRY_FRSKY_SPORT;
} }

View file

@ -419,7 +419,9 @@ const char STR_PATH_TOO_LONG[] = TR_PATH_TOO_LONG;
const char STR_VIEW_TEXT[] = TR_VIEW_TEXT; const char STR_VIEW_TEXT[] = TR_VIEW_TEXT;
const char STR_FLASH_BOOTLOADER[] = TR_FLASH_BOOTLOADER; const char STR_FLASH_BOOTLOADER[] = TR_FLASH_BOOTLOADER;
const char STR_FLASH_INTERNAL_MODULE[] = TR_FLASH_INTERNAL_MODULE; const char STR_FLASH_INTERNAL_MODULE[] = TR_FLASH_INTERNAL_MODULE;
const char STR_FLASH_INTERNAL_MULTI[] = TR_FLASH_INTERNAL_MULTI;
const char STR_FLASH_EXTERNAL_MODULE[] = TR_FLASH_EXTERNAL_MODULE; const char STR_FLASH_EXTERNAL_MODULE[] = TR_FLASH_EXTERNAL_MODULE;
const char STR_FLASH_EXTERNAL_MULTI[] = TR_FLASH_EXTERNAL_MULTI;
const char STR_FIRMWARE_UPDATE_ERROR[] = TR_FIRMWARE_UPDATE_ERROR; const char STR_FIRMWARE_UPDATE_ERROR[] = TR_FIRMWARE_UPDATE_ERROR;
const char STR_FIRMWARE_UPDATE_SUCCESS[] = TR_FIRMWARE_UPDATE_SUCCESS; const char STR_FIRMWARE_UPDATE_SUCCESS[] = TR_FIRMWARE_UPDATE_SUCCESS;
const char STR_WRITING[] = TR_WRITING; const char STR_WRITING[] = TR_WRITING;

View file

@ -623,7 +623,9 @@ extern const char STR_FLASH_BLUETOOTH_MODULE[];
extern const char STR_FLASH_POWER_MANAGEMENT_UNIT[]; extern const char STR_FLASH_POWER_MANAGEMENT_UNIT[];
extern const char STR_CURRENT_VERSION[]; extern const char STR_CURRENT_VERSION[];
extern const char STR_FLASH_INTERNAL_MODULE[]; extern const char STR_FLASH_INTERNAL_MODULE[];
extern const char STR_FLASH_INTERNAL_MULTI[];
extern const char STR_FLASH_EXTERNAL_MODULE[]; extern const char STR_FLASH_EXTERNAL_MODULE[];
extern const char STR_FLASH_EXTERNAL_MULTI[];
extern const char STR_FIRMWARE_UPDATE_ERROR[]; extern const char STR_FIRMWARE_UPDATE_ERROR[];
extern const char STR_FIRMWARE_UPDATE_SUCCESS[]; extern const char STR_FIRMWARE_UPDATE_SUCCESS[];
extern const char STR_WRITING[]; extern const char STR_WRITING[];

View file

@ -822,7 +822,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT TR("Flash řízení spotř.", "Flash jednotky řízení spotřeby") #define TR_FLASH_POWER_MANAGEMENT_UNIT TR("Flash řízení spotř.", "Flash jednotky řízení spotřeby")
#define TR_CURRENT_VERSION TR("Současná ver. ", "Současná verze: ") #define TR_CURRENT_VERSION TR("Současná ver. ", "Současná verze: ")
#define TR_FLASH_INTERNAL_MODULE TR("Flash vnitř. modulu", "Flash vnitřního modulu") #define TR_FLASH_INTERNAL_MODULE TR("Flash vnitř. modulu", "Flash vnitřního modulu")
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. modulu", "Flash externího zařízení") #define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. modulu", "Flash externího zařízení")
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("Chyba zápisu FW", "Chyba zápisu firmware") #define TR_FIRMWARE_UPDATE_ERROR TR("Chyba zápisu FW", "Chyba zápisu firmware")
#define TR_FIRMWARE_UPDATE_SUCCESS "Úspěšný zápis FW" #define TR_FIRMWARE_UPDATE_SUCCESS "Úspěšný zápis FW"
#define TR_WRITING TR3("\14Zapisuji..", "\032Zapisuji..", "Zapisuji..") #define TR_WRITING TR3("\14Zapisuji..", "\032Zapisuji..", "Zapisuji..")

View file

@ -824,7 +824,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT TR("Flash pwr mngt unit", "Flash power management unit") #define TR_FLASH_POWER_MANAGEMENT_UNIT TR("Flash pwr mngt unit", "Flash power management unit")
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE TR("Flash int. XJT","Flash internes XJT-Modul") #define TR_FLASH_INTERNAL_MODULE TR("Flash int. XJT","Flash internes XJT-Modul")
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. mod","Flash extern module") #define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. mod","Flash extern module")
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "Writing..." #define TR_WRITING "Writing..."

View file

@ -825,7 +825,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Curr Vers: ", "Current version: ") #define TR_CURRENT_VERSION TR("Curr Vers: ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE TR("Flash int. module", "Flash internal module") #define TR_FLASH_INTERNAL_MODULE TR("Flash int. module", "Flash internal module")
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. module", "Flash external module") #define TR_FLASH_EXTERNAL_MODULE TR("Flash ext. module", "Flash external module")
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update error", "Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update error", "Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "Writing..." #define TR_WRITING "Writing..."

View file

@ -847,7 +847,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Flash Internal Module" #define TR_FLASH_INTERNAL_MODULE "Flash Internal Module"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "Writing..." #define TR_WRITING "Writing..."

View file

@ -841,7 +841,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Flash Internal Module" #define TR_FLASH_INTERNAL_MODULE "Flash Internal Module"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "Writing..." #define TR_WRITING "Writing..."

View file

@ -843,7 +843,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION "Version courante :" #define TR_CURRENT_VERSION "Version courante :"
#define TR_FLASH_INTERNAL_MODULE TR("Flasher module int.", "Flasher module interne") #define TR_FLASH_INTERNAL_MODULE TR("Flasher module int.", "Flasher module interne")
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE TR("Flasher module ext.", "Flasher module externe") #define TR_FLASH_EXTERNAL_MODULE TR("Flasher module ext.", "Flasher module externe")
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("Erreur màj FW","Erreur de mise à jour") #define TR_FIRMWARE_UPDATE_ERROR TR("Erreur màj FW","Erreur de mise à jour")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash ok" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash ok"
#define TR_WRITING TR("\14Ecriture...", "\032Ecriture...") #define TR_WRITING TR("\14Ecriture...", "\032Ecriture...")

View file

@ -842,7 +842,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Progr. Modulo Interno" #define TR_FLASH_INTERNAL_MODULE "Progr. Modulo Interno"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "\032Scrivendo..." #define TR_WRITING "\032Scrivendo..."

View file

@ -830,7 +830,9 @@ TR_GYR_VSRCRAW
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Flash interne XJT-Module" #define TR_FLASH_INTERNAL_MODULE "Flash interne XJT-Module"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "\032Schrijven..." #define TR_WRITING "\032Schrijven..."

View file

@ -842,7 +842,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Sflashuj Moduł Wewnętrzny" #define TR_FLASH_INTERNAL_MODULE "Sflashuj Moduł Wewnętrzny"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "\032Zapis... " #define TR_WRITING "\032Zapis... "

View file

@ -832,7 +832,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Flash Internal Module" #define TR_FLASH_INTERNAL_MODULE "Flash Internal Module"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "Writing..." #define TR_WRITING "Writing..."

View file

@ -842,7 +842,9 @@
#define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit" #define TR_FLASH_POWER_MANAGEMENT_UNIT "Flash pwr mngt unit"
#define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ") #define TR_CURRENT_VERSION TR("Current vers. ", "Current version: ")
#define TR_FLASH_INTERNAL_MODULE "Flash Internal Module" #define TR_FLASH_INTERNAL_MODULE "Flash Internal Module"
#define TR_FLASH_INTERNAL_MULTI TR("Flash Int. Multi", "Flash Internal Multi")
#define TR_FLASH_EXTERNAL_MODULE "Flash external module" #define TR_FLASH_EXTERNAL_MODULE "Flash external module"
#define TR_FLASH_EXTERNAL_MULTI TR("Flash Ext. Multi", "Flash External Multi")
#define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error") #define TR_FIRMWARE_UPDATE_ERROR TR("FW update Error","Firmware update error")
#define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful" #define TR_FIRMWARE_UPDATE_SUCCESS "Flash successful"
#define TR_WRITING "\032Skriver..." #define TR_WRITING "\032Skriver..."

View file

@ -139,6 +139,11 @@ def main():
cmake_options["PCBREV"] = "T12" cmake_options["PCBREV"] = "T12"
firmware_options = options_taranis_x9dp firmware_options = options_taranis_x9dp
maxsize = 65536 * 8 maxsize = 65536 * 8
elif board_name == "t16":
cmake_options["PCB"] = "X10"
cmake_options["PCBREV"] = "T16"
firmware_options = options_jumper_t16
maxsize = 2 * 1024 * 1024
else: else:
exit(INVALID_BOARD) exit(INVALID_BOARD)

View file

@ -198,3 +198,16 @@ options_horus_x10express = {
"eu": ("MODULE_PROTOCOL_D8", "NO", "YES"), "eu": ("MODULE_PROTOCOL_D8", "NO", "YES"),
"flexr9m": ("MODULE_PROTOCOL_FLEX", "YES", None) "flexr9m": ("MODULE_PROTOCOL_FLEX", "YES", None)
} }
options_jumper_t16 = {
"noheli": ("HELI", "NO", "YES"),
"ppmus": ("PPM_UNIT", "US", "PERCENT_PREC1"),
"lua": ("LUA", "YES", "NO_MODEL_SCRIPTS"),
"nogvars": ("GVARS", "NO", "YES"),
"faimode": ("FAI", "YES", None),
"faichoice": ("FAI", "CHOICE", None),
"nooverridech": ("OVERRIDE_CHANNEL_FUNCTION", "NO", "YES"),
"eu": ("MODULE_PROTOCOL_D8", "NO", "YES"),
"flexr9m": ("MODULE_PROTOCOL_FLEX", "YES", None),
"internalmulti": ("INTERNAL_MODULE_MULTI", "YES", "NO"),
}