piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

[X9D] Switches working on simu

Browse Source 
[stock / gruvin9x] N-pass mixer
This commit is contained in:
bsongis
2012-12-04 14:26:41 +00:00
parent 968d1307fc
commit ce7adabfbe
9 changed files with 659 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/board_sky9x.cpp
+64 -10
View File
@@ -898,49 +898,103 @@ uint8_t keyDown()
#if defined(PCBX9D)
extern uint32_t keyState(EnumKeys enuk)
{
register uint32_t a ;
register uint32_t c ;
CPU_UINT xxx = 0;
if (enuk < (int) DIM(keys)) return keys[enuk].state() ? 1 : 0;
a = PIOA->PIO_PDSR ;
c = PIOC->PIO_PDSR ;
// printf("c=%x\n", c); fflush(stdout);
switch ((uint8_t) enuk) {
case SW_SA0:
xxx = c & 0x80000000;
xxx = !(c & 0x80000000);
break;
case SW_SA2:
xxx = c & 0x40000000;
xxx = c & 0x80000000;
break;
case SW_SB0:
xxx = c & 0x08000000;
xxx = !(c & 0x08000000) && !(c & 0x04000000);
break;
case SW_SB1:
xxx = c & 0x04000000;
xxx = (c & 0x08000000) && !(c & 0x04000000);
break;
case SW_SB2:
xxx = c & 0x02000000;
xxx = !(c & 0x08000000) && (c & 0x04000000);
break;
case SW_SC0:
xxx = c & 0x00800000;
xxx = !(c & 0x00800000) && !(c & 0x00400000);
break;
case SW_SC1:
xxx = c & 0x00400000;
xxx = (c & 0x00800000) && !(c & 0x00400000);
break;
case SW_SC2:
xxx = c & 0x00200000;
xxx = !(c & 0x00800000) && (c & 0x00400000);
break;
case SW_SD0:
xxx = !(c & 0x00200000) && !(c & 0x00100000);
break;
case SW_SD1:
xxx = (c & 0x00200000) && !(c & 0x00100000);
break;
case SW_SD2:
xxx = !(c & 0x00200000) && (c & 0x00100000);
break;
case SW_SE0:
xxx = !(c & 0x00080000) && !(c & 0x00040000);
break;
case SW_SE1:
xxx = (c & 0x00080000) && !(c & 0x00040000);
break;
case SW_SE2:
xxx = !(c & 0x00080000) && (c & 0x00040000);
break;
case SW_SF0:
xxx = !(c & 0x00020000) && !(c & 0x00010000);
break;
case SW_SF1:
xxx = (c & 0x00020000) && !(c & 0x00010000);
break;
case SW_SF2:
xxx = !(c & 0x00020000) && (c & 0x00010000);
break;
case SW_SG0:
xxx = !(c & 0x00008000) && !(c & 0x00004000);
break;
case SW_SG1:
xxx = (c & 0x00008000) && !(c & 0x00004000);
break;
case SW_SG2:
xxx = !(c & 0x00008000) && (c & 0x00004000);
break;
case SW_SH0:
xxx = !(c & 0x00002000);
break;
case SW_SH2:
xxx = c & 0x00002000;
break;
default:
src/gtests.cpp
+169 -23
View File
@@ -34,36 +34,44 @@
#include <gtest/gtest.h>
#include "open9x.h"
void doMixerCalculations();
#define MODEL_RESET() memset(&g_model, 0, sizeof(g_model))
#define MIXER_RESET() \
memset(g_chans512, 0, sizeof(g_chans512)); \
memset(ex_chans, 0, sizeof(ex_chans))
uint16_t anaIn(uint8_t chan)
{
return 0;
}
TEST(trims, greaterTrimLink)
TEST(Trims, greaterTrimLink)
{
memset(&g_model, 0, sizeof(g_model));
setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim
setTrimValue(3, 0, 32);
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(1, 0), 0), 32);
MODEL_RESET();
setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim
setTrimValue(3, 0, 32);
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(1, 0), 0), 32);
}
TEST(trims, chainedTrims)
TEST(Trims, chainedTrims)
{
memset(&g_model, 0, sizeof(g_model));
setTrimValue(0, 0, 32);
setTrimValue(1, 0, TRIM_EXTENDED_MAX+1); // link to FP0 trim
setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32);
MODEL_RESET();
setTrimValue(0, 0, 32);
setTrimValue(1, 0, TRIM_EXTENDED_MAX+1); // link to FP0 trim
setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32);
}
TEST(trims, infiniteChainedTrims)
TEST(Trims, infiniteChainedTrims)
{
memset(&g_model, 0, sizeof(g_model));
setTrimValue(0, 0, 32);
setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim
setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim
setTrimValue(3, 0, TRIM_EXTENDED_MAX+3); // link to FP2 trim
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32);
MODEL_RESET();
setTrimValue(0, 0, 32);
setTrimValue(1, 0, TRIM_EXTENDED_MAX+3); // link to FP3 trim
setTrimValue(2, 0, TRIM_EXTENDED_MAX+2); // link to FP1 trim
setTrimValue(3, 0, TRIM_EXTENDED_MAX+3); // link to FP2 trim
EXPECT_EQ(getRawTrimValue(getTrimFlightPhase(0, 2), 0), 32);
}
TEST(outdezNAtt, test_unsigned)
@@ -259,14 +267,14 @@ TEST(FrSky, dateNtime)
TEST(getSwitch, undefCSW)
{
memset(&g_model, 0, sizeof(g_model));
MODEL_RESET();
EXPECT_EQ(getSwitch(MAX_PSWITCH, 0), false);
EXPECT_EQ(getSwitch(-MAX_PSWITCH, 0), true); // no good answer there!
}
TEST(getSwitch, circularCSW)
{
memset(&g_model, 0, sizeof(g_model));
MODEL_RESET();
g_model.customSw[0] = { MAX_SWITCH-NUM_CSW, MAX_SWITCH-NUM_CSW, CS_OR };
g_model.customSw[1] = { MAX_SWITCH-NUM_CSW, MAX_SWITCH-NUM_CSW, CS_AND };
EXPECT_EQ(getSwitch(MAX_SWITCH-NUM_CSW, 0), false);
@@ -277,15 +285,15 @@ TEST(getSwitch, circularCSW)
TEST(getSwitch, nullSW)
{
memset(&g_model, 0, sizeof(g_model));
MODEL_RESET();
EXPECT_EQ(getSwitch(0, 0), false);
EXPECT_EQ(getSwitch(0, true), true);
EXPECT_EQ(getSwitch(0, 0), false);
}
TEST(phases, nullFadeOut_posFadeIn)
TEST(Phases, nullFadeOut_posFadeIn)
{
memset(&g_model, 0, sizeof(g_model));
MODEL_RESET();
g_model.phaseData[1].swtch = DSW(SW_ID1);
g_model.phaseData[1].fadeIn = 15;
perMain();
@@ -293,6 +301,144 @@ TEST(phases, nullFadeOut_posFadeIn)
perMain();
}
TEST(Mixer, Cascaded3Channels)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].srcRaw = MIXSRC_CH2;
g_model.mixData[0].weight = 100;
g_model.mixData[1].destCh = 1;
g_model.mixData[1].srcRaw = MIXSRC_CH3;
g_model.mixData[1].weight = 100;
g_model.mixData[2].destCh = 2;
g_model.mixData[2].srcRaw = MIXSRC_ID1;
g_model.mixData[2].weight = 100;
setSwitch(DSW(SW_ID1));
perOut(e_perout_mode_normal, 0);
EXPECT_EQ(chans[0], 102400);
EXPECT_EQ(chans[1], 102400);
EXPECT_EQ(chans[2], 102400);
}
TEST(Mixer, CascadedOrderedChannels)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].srcRaw = MIXSRC_ID1;
g_model.mixData[0].weight = 100;
g_model.mixData[1].destCh = 1;
g_model.mixData[1].srcRaw = MIXSRC_CH1;
g_model.mixData[1].weight = 100;
setSwitch(DSW(SW_ID1));
perOut(e_perout_mode_normal, 0);
EXPECT_EQ(chans[0], 102400);
EXPECT_EQ(chans[1], 102400);
}
TEST(Mixer, Cascaded5Channels)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].srcRaw = MIXSRC_CH2;
g_model.mixData[0].weight = 100;
g_model.mixData[1].destCh = 1;
g_model.mixData[1].srcRaw = MIXSRC_CH3;
g_model.mixData[1].weight = 100;
g_model.mixData[2].destCh = 2;
g_model.mixData[2].srcRaw = MIXSRC_CH4;
g_model.mixData[2].weight = 100;
g_model.mixData[3].destCh = 3;
g_model.mixData[3].srcRaw = MIXSRC_CH5;
g_model.mixData[3].weight = 100;
g_model.mixData[4].destCh = 4;
g_model.mixData[4].srcRaw = MIXSRC_ID1;
g_model.mixData[4].weight = 100;
int32_t lastVal = 0;
for (uint8_t i=0; i<10; i++) {
setSwitch(DSW(SW_ID1));
doMixerCalculations();
EXPECT_EQ(chans[0], lastVal);
EXPECT_EQ(chans[1], lastVal);
EXPECT_EQ(chans[2], 102400);
EXPECT_EQ(chans[3], 102400);
EXPECT_EQ(chans[4], 102400);
doMixerCalculations();
EXPECT_EQ(chans[0], 102400);
EXPECT_EQ(chans[1], 102400);
EXPECT_EQ(chans[2], 102400);
EXPECT_EQ(chans[3], 102400);
EXPECT_EQ(chans[4], 102400);
setSwitch(DSW(SW_ID0));
doMixerCalculations();
EXPECT_EQ(chans[0], 102400);
EXPECT_EQ(chans[1], 102400);
EXPECT_EQ(chans[2], -102400);
EXPECT_EQ(chans[3], -102400);
EXPECT_EQ(chans[4], -102400);
doMixerCalculations();
EXPECT_EQ(chans[0], -102400);
EXPECT_EQ(chans[1], -102400);
EXPECT_EQ(chans[2], -102400);
EXPECT_EQ(chans[3], -102400);
EXPECT_EQ(chans[4], -102400);
lastVal = -102400;
}
}
TEST(Mixer, InfiniteRecursiveChannels)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].srcRaw = MIXSRC_CH2;
g_model.mixData[0].weight = 100;
g_model.mixData[1].destCh = 1;
g_model.mixData[1].srcRaw = MIXSRC_CH3;
g_model.mixData[1].weight = 100;
g_model.mixData[2].destCh = 2;
g_model.mixData[2].srcRaw = MIXSRC_CH1;
g_model.mixData[2].weight = 100;
perOut(e_perout_mode_normal, 0);
EXPECT_EQ(chans[2], 0);
EXPECT_EQ(chans[1], 0);
EXPECT_EQ(chans[0], 0);
}
TEST(Mixer, BlockingChannel)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].srcRaw = MIXSRC_CH1;
g_model.mixData[0].weight = 100;
perOut(e_perout_mode_normal, 0);
EXPECT_EQ(chans[0], 0);
}
TEST(Mixer, RecursiveAddChannel)
{
MODEL_RESET();
MIXER_RESET();
g_model.mixData[0].destCh = 0;
g_model.mixData[0].mltpx = MLTPX_ADD;
g_model.mixData[0].srcRaw = MIXSRC_MAX;
g_model.mixData[0].weight = 50;
g_model.mixData[1].destCh = 0;
g_model.mixData[1].mltpx = MLTPX_ADD;
g_model.mixData[1].srcRaw = MIXSRC_CH2;
g_model.mixData[1].weight = 100;
g_model.mixData[2].destCh = 1;
g_model.mixData[2].srcRaw = MIXSRC_Rud;
g_model.mixData[2].weight = 100;
perOut(e_perout_mode_normal, 0);
EXPECT_EQ(chans[0], 102400/2);
EXPECT_EQ(chans[1], 0);
}
int main(int argc, char **argv) {
StartEepromThread(NULL);
g_menuStackPtr = 0;
src/main_views.cpp
+4 -4
View File
@@ -433,15 +433,15 @@ void menuMainView(uint8_t event)
uint8_t sw;
switch(i) {
case 0:
sw = getValue(MIXSRC_SA) > 0 ? 2 : 1;
sw = getValue(MIXSRC_SA-1) > 0 ? 2 : 1;
break;
case 7:
sw = getValue(MIXSRC_SH) > 0 ? 2+2+3*6 : 1+3*6;
sw = getValue(MIXSRC_SH-1) > 0 ? 2+2+3*6 : 1+2+3*6;
break;
default:
{
int8_t val = getValue(MIXSRC_SA+i);
sw = ((val > 0) ? 3*i+2 : ((val < 0) ? 3*i+1 : 3*i));
int16_t val = getValue(MIXSRC_SA+i-1);
sw = ((val < 0) ? 3*i : ((val == 0) ? 3*i+1 : 3*i+2));
break;
}
}
src/menus.cpp
+1 -1
View File
@@ -164,7 +164,7 @@ int16_t checkIncDec(uint8_t event, int16_t val, int16_t i_min, int16_t i_max, ui
}
if (newval != val) {
if ((~i_flags & NO_INCDEC_MARKS) && (newval != i_max) && (newval != i_min) && (newval==0 || newval==-100 || newval==+100)) {
if (!(i_flags & NO_INCDEC_MARKS) && (newval != i_max) && (newval != i_min) && (newval==0 || newval==-100 || newval==+100)) {
pauseEvents(event); // delay before auto-repeat continues
if (newval>val) // without AUDIO it's optimized, because the 2 sounds are the same
AUDIO_KEYPAD_UP();
src/myeeprom.h
+1 -1
View File
@@ -669,7 +669,7 @@ enum MixSources {
MIXSRC_P2,
MIXSRC_P3,
#endif
#if defined(PCBSKY9X)
#if defined(PCBSKY9X) && !defined(PCBX9D)
MIXSRC_REa,
#elif defined(PCBGRUVIN9X)
MIXSRC_REa,
src/open9x.cpp
+210 -170
View File
@@ -88,7 +88,7 @@ const pm_uchar asterisk_lbm[] PROGMEM = {
EEGeneral g_eeGeneral;
ModelData g_model;
#if defined(PCBX9D)
#if defined(PCBX9D) && defined(SDCARD)
uint8_t modelBitmap[64*32/8];
void loadModelBitmap()
{
@@ -619,9 +619,14 @@ int16_t getValue(uint8_t i)
else if(i<MIXSRC_TrimAil) return calc1000toRESX((int16_t)8 * getTrimValue(s_perout_flight_phase, i-(NUM_STICKS+NUM_POTS+NUM_ROTARY_ENCODERS)));
else if(i<MIXSRC_MAX) return 1024;
#if defined(PCBX9D)
else if(i==MIXSRC_SA) return (keyState(SW_SA0) ? -1024 : 1024);
else if(i==MIXSRC_SB) return (keyState(SW_SB0) ? -1024 : (keyState(SW_SB1) ? 0 : 1024));
else if(i==MIXSRC_SC) return (keyState(SW_SC0) ? -1024 : (keyState(SW_SC1) ? 0 : 1024));
else if(i<MIXSRC_SA) return (keyState(SW_SA0) ? -1024 : 1024);
else if(i<MIXSRC_SB) return (keyState(SW_SB0) ? -1024 : (keyState(SW_SB1) ? 0 : 1024));
else if(i<MIXSRC_SC) return (keyState(SW_SC0) ? -1024 : (keyState(SW_SC1) ? 0 : 1024));
else if(i<MIXSRC_SD) return (keyState(SW_SD0) ? -1024 : (keyState(SW_SD1) ? 0 : 1024));
else if(i<MIXSRC_SE) return (keyState(SW_SE0) ? -1024 : (keyState(SW_SE1) ? 0 : 1024));
else if(i<MIXSRC_SF) return (keyState(SW_SF0) ? -1024 : (keyState(SW_SF1) ? 0 : 1024));
else if(i<MIXSRC_SG) return (keyState(SW_SG0) ? -1024 : (keyState(SW_SG1) ? 0 : 1024));
else if(i<MIXSRC_SH) return (keyState(SW_SH0) ? -1024 : 1024);
#else
else if(i<MIXSRC_3POS) return (keyState(SW_ID0) ? -1024 : (keyState(SW_ID1) ? 0 : 1024));
// here the switches are skipped
@@ -1100,7 +1105,7 @@ void putsTelemetryValue(uint8_t x, uint8_t y, int16_t val, uint8_t unit, uint8_t
}
#endif
lcd_outdezAtt(x, (att & DBLSIZE ? y - FH : y), val, att & (~NO_UNIT));
if (~att & NO_UNIT && unit != UNIT_RAW)
if (!(att & NO_UNIT) && unit != UNIT_RAW)
lcd_putsiAtt(lcdLastPos/*+1*/, y, STR_VTELEMUNIT, unit, 0);
}
#endif
@@ -2071,7 +2076,7 @@ void evalFunctions()
safetyCh[sd->func] = FSW_PARAM(sd);
}
if (~activeFunctions & function_mask) {
if (!(activeFunctions & function_mask)) {
if (sd->func == FUNC_INSTANT_TRIM) {
if (g_menuStack[0] == menuMainView
#if defined(FRSKY)
@@ -2092,7 +2097,7 @@ void evalFunctions()
}
#endif
if (~activeFunctionSwitches & switch_mask) {
if (!(activeFunctionSwitches & switch_mask)) {
if (sd->func == FUNC_RESET) {
switch (FSW_PARAM(sd)) {
case 0:
@@ -2120,7 +2125,7 @@ void evalFunctions()
else
#endif
if (!COMPLEX_SWITCH || (~activeFunctionSwitches & switch_mask)) {
if (!COMPLEX_SWITCH || !(activeFunctionSwitches & switch_mask)) {
if (sd->func == FUNC_PLAY_SOUND) {
AUDIO_PLAY(AU_FRSKY_FIRST+FSW_PARAM(sd));
}
@@ -2201,8 +2206,8 @@ void evalFunctions()
}
if (COMPLEX_SWITCH) {
if (~activeFunctionSwitches & switch_mask) {
if (~activeFunctions & function_mask) {
if (!(activeFunctionSwitches & switch_mask)) {
if (!(activeFunctions & function_mask)) {
newActiveFunctions |= function_mask;
}
}
@@ -2317,202 +2322,231 @@ void perOut(uint8_t mode, uint8_t tick10ms)
}
#endif
memclear(chans, sizeof(chans)); // All outputs to 0
//========== MIXER LOOP ===============
uint8_t lv_mixWarning = 0;
for (uint8_t i=0; i<MAX_MIXERS; i++) {
uint8_t pass = 0;
MixData *md = mixaddress( i ) ;
bitfield_channels_t dirtyChannels = (bitfield_channels_t)-1; // all dirty when mixer starts
if (md->srcRaw==0) break;
do {
if (md->phases & (1<<s_perout_flight_phase)) continue;
// printf("[pass %d]\n", pass); fflush(stdout);
//========== SWITCH ===============
bool sw = getSwitch(md->swtch, 1);
bitfield_channels_t passDirtyChannels = 0;
//========== VALUE ===============
//Notice 0 = NC switch means not used -> always on line
uint8_t k = md->srcRaw-1;
int16_t v = 0;
if (mode != e_perout_mode_normal) {
if (!sw || k >= NUM_STICKS || (k == THR_STICK && g_model.thrTrim)) {
continue;
for (uint8_t i = 0; i < MAX_MIXERS; i++) {
MixData *md = mixaddress(i);
if (md->srcRaw == 0) break;
uint8_t k = md->srcRaw - 1;
if (!(dirtyChannels & ((bitfield_channels_t) 1 << md->destCh))) continue;
if (md->phases & (1 << s_perout_flight_phase)) continue;
//========== SWITCH ===============
bool sw = getSwitch(md->swtch, 1);
//========== VALUE ===============
int16_t v = 0;
if (mode != e_perout_mode_normal) {
if (!sw || k >= NUM_STICKS || (k == THR_STICK && g_model.thrTrim)) {
continue;
}
else {
if (!(mode & e_perout_mode_nosticks)) v = anas[k];
}
}
else {
if (!(mode & e_perout_mode_nosticks))
v = anas[k];
}
}
else {
if (k < NUM_STICKS)
v = md->noExpo ? rawAnas[k] : anas[k]; //Switch is on. MAX=FULL=512 or value.
else if (k>=MIXSRC_CH1-1 && k<=MIXSRC_CH16-1 && k-MIXSRC_CH1+1<md->destCh) // if we've already calculated the value - take it instead
v = chans[k-MIXSRC_CH1+1] / 100;
if (k < NUM_STICKS)
v = md->noExpo ? rawAnas[k] : anas[k]; //Switch is on. MAX=FULL=512 or value.
#if defined(PCBX9D)
// TODO
// TODO
#else
else if (k>=MIXSRC_THR-1 && k<=MIXSRC_SWC-1) {
v = getSwitch(k-MIXSRC_THR+1+1, 0) ? +1024 : -1024;
if (v<0 && !md->swtch)
sw = false;
else if (k >= MIXSRC_THR - 1 && k <= MIXSRC_SWC - 1) {
v = getSwitch(k - MIXSRC_THR + 1 + 1, 0) ? +1024 : -1024;
if (v < 0 && !md->swtch) sw = false;
}
else {
v = getValue(k <= MIXSRC_3POS ? k : k - MAX_SWITCH);
if (k >= MIXSRC_CH1 - 1 && k <= MIXSRC_CHMAX - 1
&& md->destCh != k - MIXSRC_CH1 + 1) {
if (dirtyChannels
& ((bitfield_channels_t) 1 << (k - MIXSRC_CH1 + 1))
& (passDirtyChannels
| ~(((bitfield_channels_t) 1 << md->destCh) - 1))) passDirtyChannels |=
(bitfield_channels_t) 1 << md->destCh;
if (k - MIXSRC_CH1 + 1 < md->destCh || pass > 0) v = chans[k
- MIXSRC_CH1 + 1] / 100;
}
}
#endif
}
//========== DELAYS ===============
uint8_t swTog;
bool apply_offset = true;
if (sw) { // switch on? (if no switch selected => on)
swTog = !swOn[i];
if (mode == e_perout_mode_normal) {
swOn[i] = true;
if (md->delayUp) {
if (swTog) {
if (sDelay[i])
sDelay[i] = 0;
else
sDelay[i] = md->delayUp * 50;
}
if (sDelay[i] > 0) { // perform delay
sDelay[i] = max(0, sDelay[i] - tick10ms);
if (!md->swtch) {
v = -1024;
}
else {
continue;
}
}
}
if (md->mixWarn) lv_mixWarning |= 1 << (md->mixWarn - 1); // Mix warning
}
}
else {
v = getValue(k <= MIXSRC_3POS ? k : k-MAX_SWITCH);
}
#endif
}
//========== DELAYS ===============
uint8_t swTog;
bool apply_offset = true;
if (sw) { // switch on? (if no switch selected => on)
swTog = !swOn[i];
if (mode == e_perout_mode_normal) {
swOn[i] = true;
if (md->delayUp) {
bool has_delay = false;
swTog = swOn[i];
swOn[i] = false;
if (md->delayDown) {
if (swTog) {
if (sDelay[i])
sDelay[i] = 0;
else
sDelay[i] = md->delayUp * 50;
sDelay[i] = md->delayDown * 50;
}
if (sDelay[i] > 0) { // perform delay
sDelay[i] = max(0, sDelay[i]-tick10ms);
if (!md->swtch) {
v = -1024;
}
else {
continue;
}
sDelay[i] = max(0, sDelay[i] - tick10ms);
if (!md->swtch) v = +1024;
has_delay = true;
}
else if (!md->swtch) {
v = -1024;
}
}
if (md->mixWarn) lv_mixWarning |= 1<<(md->mixWarn-1); // Mix warning
}
}
else {
bool has_delay = false;
swTog = swOn[i];
swOn[i] = false;
if (md->delayDown) {
if (swTog) {
if (sDelay[i])
sDelay[i] = 0;
else
sDelay[i] = md->delayDown * 50;
}
if (sDelay[i] > 0) { // perform delay
sDelay[i] = max(0, sDelay[i]-tick10ms);
if (!md->swtch) v = +1024;
has_delay = true;
}
else if (!md->swtch) {
v = -1024;
if (!has_delay) {
if (md->speedDown) {
if (md->mltpx == MLTPX_REP) continue;
if (md->swtch) {
v = 0;
apply_offset = false;
}
}
else if (md->swtch) {
continue;
}
}
}
if (!has_delay) {
if (md->speedDown) {
if (md->mltpx==MLTPX_REP) continue;
if (md->swtch) { v = 0; apply_offset = false; }
}
else if (md->swtch) {
continue;
}
}
}
#ifdef BOLD_FONT
activeMixes |= ((ACTIVE_MIXES_TYPE)1 << i);
activeMixes |= ((ACTIVE_MIXES_TYPE) 1 << i);
#endif
//========== OFFSET ===============
if (apply_offset) {
int8_t offset = GET_GVAR(md->offset, -125, 125, s_perout_flight_phase);
if (offset) v += calc100toRESX(offset);
}
//========== OFFSET ===============
if (apply_offset) {
int8_t offset = GET_GVAR(md->offset, -125, 125, s_perout_flight_phase);
if (offset) v += calc100toRESX(offset);
}
//========== TRIMS ===============
if (!(mode & e_perout_mode_notrims)) {
int8_t mix_trim = md->carryTrim;
if (mix_trim < TRIM_ON)
mix_trim = -mix_trim-1;
else if (mix_trim == TRIM_ON && k < NUM_STICKS)
mix_trim = k;
else
mix_trim = -1;
if (mix_trim >= 0)
v += trims[mix_trim];
}
//========== TRIMS ===============
if (!(mode & e_perout_mode_notrims)) {
int8_t mix_trim = md->carryTrim;
if (mix_trim < TRIM_ON)
mix_trim = -mix_trim - 1;
else if (mix_trim == TRIM_ON && k < NUM_STICKS)
mix_trim = k;
else
mix_trim = -1;
if (mix_trim >= 0) v += trims[mix_trim];
}
int16_t weight = GET_GVAR(md->weight, -500, 500, s_perout_flight_phase);
int16_t weight = GET_GVAR(md->weight, -500, 500, s_perout_flight_phase);
//========== SPEED ===============
if (mode == e_perout_mode_normal && (md->speedUp || md->speedDown)) // there are delay values
{
//========== SPEED ===============
if (mode == e_perout_mode_normal && (md->speedUp || md->speedDown)) { // there are delay values
#define DEL_MULT 256
int16_t diff = v-act[i]/DEL_MULT;
int16_t diff = v - act[i] / DEL_MULT;
if (diff) {
//rate = steps/sec => 32*1024/100*md->speedUp/Down
//act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown);
//-100..100 => 32768 -> 100*83886/256 = 32768, For MAX we divide by 2 since it's asymmetrical
if (tick10ms) {
int32_t rate = (int32_t)DEL_MULT*2048*100*tick10ms;
if (diff) {
//rate = steps/sec => 32*1024/100*md->speedUp/Down
//act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown);
//-100..100 => 32768 -> 100*83886/256 = 32768, For MAX we divide by 2 since it's asymmetrical
if (tick10ms) {
int32_t rate = (int32_t) DEL_MULT * 2048 * 100 * tick10ms;
if (weight) rate /= abs(weight);
act[i] = (diff>0) ? ((md->speedUp>0) ? act[i]+(rate)/((int16_t)50*md->speedUp) : (int32_t)v*DEL_MULT) :
((md->speedDown>0) ? act[i]-(rate)/((int16_t)50*md->speedDown) : (int32_t)v*DEL_MULT) ;
}
}
{
int32_t tmp = act[i]/DEL_MULT ;
if(((diff>0) && (v<tmp)) || ((diff<0) && (v>tmp))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow
}
{
int32_t tmp = act[i] / DEL_MULT;
if (((diff > 0) && (v < tmp)) || ((diff < 0) && (v > tmp))) act[i] = (int32_t) v * DEL_MULT; //deal with overflow
}
v = act[i]/DEL_MULT;
v = act[i] / DEL_MULT;
}
}
}
//========== CURVES ===============
if (md->curveParam && md->curveMode == MODE_CURVE) {
v = applyCurve(v, md->curveParam);
}
//========== CURVES ===============
if (md->curveParam && md->curveMode == MODE_CURVE) {
v = applyCurve(v, md->curveParam);
}
//========== WEIGHT ===============
int32_t dv = (int32_t)v*weight;
//========== WEIGHT ===============
int32_t dv = (int32_t) v * weight;
//========== DIFFERENTIAL =========
if (md->curveMode == MODE_DIFFERENTIAL) {
int8_t curveParam = GET_GVAR(md->curveParam, -100, 100, s_perout_flight_phase);
if (curveParam>0 && dv<0)
dv = (dv * (100-curveParam)) / 100;
else if (curveParam<0 && dv>0)
dv = (dv * (100+curveParam)) / 100;
}
//========== DIFFERENTIAL =========
if (md->curveMode == MODE_DIFFERENTIAL) {
int8_t curveParam =
GET_GVAR(md->curveParam, -100, 100, s_perout_flight_phase);
if (curveParam > 0 && dv < 0)
dv = (dv * (100 - curveParam)) / 100;
else if (curveParam < 0 && dv > 0) dv = (dv * (100 + curveParam)) / 100;
}
int32_t *ptr = &chans[md->destCh]; // Save calculating address several times
switch(md->mltpx){
case MLTPX_REP:
*ptr = dv;
int32_t *ptr = &chans[md->destCh]; // Save calculating address several times
if (i == 0 || md->destCh != (md - 1)->destCh) {
*ptr = 0;
}
switch (md->mltpx) {
case MLTPX_REP:
*ptr = dv;
#ifdef BOLD_FONT
for (uint8_t m=i-1; m<MAX_MIXERS && mixaddress(m)->destCh == md->destCh; m--)
activeMixes &= ~((ACTIVE_MIXES_TYPE)1 << m);
for (uint8_t m = i - 1;
m < MAX_MIXERS && mixaddress(m)->destCh == md->destCh; m--)
activeMixes &= ~((ACTIVE_MIXES_TYPE) 1 << m);
#endif
break;
case MLTPX_MUL:
dv /= 100;
dv *= *ptr;
dv /= RESXl;
*ptr = dv;
break;
default: // MLTPX_ADD
*ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
break;
break;
case MLTPX_MUL:
dv /= 100;
dv *= *ptr;
dv /= RESXl;
*ptr = dv;
break;
default: // MLTPX_ADD
*ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
break;
}
}
}
tick10ms = 0;
dirtyChannels &= passDirtyChannels;
} while (++pass < 3 && dirtyChannels);
mixWarning = lv_mixWarning;
}
@@ -2529,9 +2563,9 @@ ACTIVE_MIXES_TYPE activeMixes;
int32_t sum_chans512[NUM_CHNOUT] = {0};
#if defined(PCBSKY9X)
inline bool doMixerCalculations()
FORCEINLINE bool doMixerCalculations()
#else
inline void doMixerCalculations()
FORCEINLINE void doMixerCalculations()
#endif
{
#if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE)
@@ -2584,14 +2618,16 @@ inline void doMixerCalculations()
}
else {
uint8_t fadeTime = max(g_model.phaseData[s_last_phase].fadeOut, g_model.phaseData[phase].fadeIn);
uint8_t transitionMask = (1<<s_last_phase) + (1<<phase);
if (fadeTime) {
s_fade_flight_phases |= (1<<s_last_phase) + (1<<phase);
s_fade_flight_phases |= transitionMask;
delta = (MAX_ACT / 50) / fadeTime;
delta *= tick10ms;
}
else {
s_fade_flight_phases &= ~transitionMask;
fp_act[s_last_phase] = 0;
fp_act[phase] = MAX_ACT;
s_fade_flight_phases &= ~((1<<s_last_phase) + (1<<phase));
}
}
s_last_phase = phase;
@@ -2618,7 +2654,7 @@ inline void doMixerCalculations()
}
//========== LIMITS ===============
for (uint8_t i=0;i<NUM_CHNOUT;i++) {
for (uint8_t i=0; i<NUM_CHNOUT; i++) {
// chans[i] holds data from mixer. chans[i] = v*weight => 1024*100
// later we multiply by the limit (up to 100) and then we need to normalize
// at the end chans[i] = chans[i]/100 => -1024..1024
@@ -2627,6 +2663,7 @@ inline void doMixerCalculations()
int32_t q = (s_fade_flight_phases ? (sum_chans512[i] / weight) * 16 : chans[i]);
ex_chans[i] = q / 100; // for the next perMain
int16_t value = applyLimits(i, q);
cli();
@@ -2817,21 +2854,24 @@ inline void doMixerCalculations()
if (s_fade_flight_phases) {
for (uint8_t p=0; p<MAX_PHASES; p++) {
if (s_fade_flight_phases & (1<<p)) {
uint8_t phaseMask = (1<<p);
if (s_fade_flight_phases & phaseMask) {
if (p == phase) {
if (MAX_ACT - fp_act[p] > delta * tick10ms)
fp_act[p] += delta * tick10ms;
if (MAX_ACT - fp_act[p] > delta)
fp_act[p] += delta;
else {
fp_act[p] = MAX_ACT;
s_fade_flight_phases -= (1<<p);
s_fade_flight_phases -= phaseMask;
}
}
else {
if (fp_act[p] > delta * tick10ms)
fp_act[p] -= delta * tick10ms;
if (fp_act[p] > delta)
fp_act[p] -= delta;
else {
fp_act[p] = 0;
s_fade_flight_phases -= (1<<p);
s_fade_flight_phases -= phaseMask;
}
}
}
src/open9x.h
+6
View File
@@ -767,6 +767,12 @@ enum PerOutMode {
extern uint8_t s_perout_flight_phase;
#if defined(CPUARM)
#define bitfield_channels_t uint32_t
#else
#define bitfield_channels_t uint16_t
#endif
void perOut(uint8_t mode, uint8_t tick10ms);
void perMain();
NOINLINE void per10ms();
src/simpgmspace.cpp
+153 -18
View File
@@ -63,44 +63,179 @@ extern const char* eeprom_buffer_data;
uint8_t eeprom[EESIZE];
sem_t *eeprom_write_sem;
#if defined(PCBSKY9X)
#define SWITCH_CASE(swtch, pin, bit) \
case -DSW(swtch): \
pin &= ~(1<<bit); \
break; \
case DSW(swtch): \
pin |= (1<<bit); \
break;
#else
#define SWITCH_CASE(swtch, pin, bit) \
case DSW(swtch): \
pin &= ~(1<<bit); \
break; \
case -DSW(swtch): \
pin |= (1<<bit); \
break;
#endif
void setSwitch(int8_t swtch)
{
switch (swtch) {
#if defined(PCBX9D)
case DSW(SW_SA0):
PIOC->PIO_PDSR &= ~(1<<31);
break;
case DSW(SW_SA2):
PIOC->PIO_PDSR |= (1<<31);
break;
case DSW(SW_SB0):
PIOC->PIO_PDSR &= ~(1<<26);
PIOC->PIO_PDSR &= ~(1<<27);
break;
case DSW(SW_SB1):
PIOC->PIO_PDSR |= (1<<27);
PIOC->PIO_PDSR &= ~(1<<26);
break;
case DSW(SW_SB2):
PIOC->PIO_PDSR |= (1<<26);
PIOC->PIO_PDSR &= ~(1<<27);
break;
case DSW(SW_SC0):
PIOC->PIO_PDSR &= ~(1<<22);
PIOC->PIO_PDSR &= ~(1<<23);
break;
case DSW(SW_SC1):
PIOC->PIO_PDSR |= (1<<23);
PIOC->PIO_PDSR &= ~(1<<22);
break;
case DSW(SW_SC2):
PIOC->PIO_PDSR |= (1<<22);
PIOC->PIO_PDSR &= ~(1<<23);
break;
case DSW(SW_SD0):
PIOC->PIO_PDSR &= ~(1<<20);
PIOC->PIO_PDSR &= ~(1<<21);
break;
case DSW(SW_SD1):
PIOC->PIO_PDSR |= (1<<21);
PIOC->PIO_PDSR &= ~(1<<20);
break;
case DSW(SW_SD2):
PIOC->PIO_PDSR |= (1<<20);
PIOC->PIO_PDSR &= ~(1<<21);
break;
case DSW(SW_SE0):
PIOC->PIO_PDSR &= ~(1<<18);
PIOC->PIO_PDSR &= ~(1<<19);
break;
case DSW(SW_SE1):
PIOC->PIO_PDSR |= (1<<19);
PIOC->PIO_PDSR &= ~(1<<18);
break;
case DSW(SW_SE2):
PIOC->PIO_PDSR |= (1<<18);
PIOC->PIO_PDSR &= ~(1<<19);
break;
case DSW(SW_SF0):
PIOC->PIO_PDSR &= ~(1<<16);
PIOC->PIO_PDSR &= ~(1<<17);
break;
case DSW(SW_SF1):
PIOC->PIO_PDSR |= (1<<17);
PIOC->PIO_PDSR &= ~(1<<16);
break;
case DSW(SW_SF2):
PIOC->PIO_PDSR |= (1<<16);
PIOC->PIO_PDSR &= ~(1<<17);
break;
case DSW(SW_SG0):
PIOC->PIO_PDSR &= ~(1<<14);
PIOC->PIO_PDSR &= ~(1<<15);
break;
case DSW(SW_SG1):
PIOC->PIO_PDSR |= (1<<15);
PIOC->PIO_PDSR &= ~(1<<14);
break;
case DSW(SW_SG2):
PIOC->PIO_PDSR |= (1<<14);
PIOC->PIO_PDSR &= ~(1<<15);
break;
case DSW(SW_SH0):
PIOC->PIO_PDSR &= ~(1<<13);
break;
case DSW(SW_SH2):
PIOC->PIO_PDSR |= (1<<13);
break;
#elif defined(PCBSKY9X)
case DSW(SW_ID0):
#if defined(PCBSKY9X)
PIOC->PIO_PDSR &= ~0x00004000;
PIOC->PIO_PDSR |= 0x00000800;
#elif defined(PCBGRUVIN9X)
ping |= (1<<INP_G_ID1);
pinb &= ~(1<<INP_B_ID2);
#else
ping |= (1<<INP_G_ID1);
pine &= ~(1<<INP_E_ID2);
#endif
break;
case DSW(SW_ID1):
#if defined(PCBSKY9X)
PIOC->PIO_PDSR |= 0x00004800;
#elif defined(PCBGRUVIN9X)
ping &= ~(1<<INP_G_ID1);
pinb &= ~(1<<INP_B_ID2);
#else
ping &= ~(1<<INP_G_ID1);
pine &= ~(1<<INP_E_ID2);
#endif
break;
case DSW(SW_ID2):
#if defined(PCBSKY9X)
PIOC->PIO_PDSR &= ~0x00000800;
PIOC->PIO_PDSR |= 0x00004000;
break;
SWITCH_CASE(SW_THR, PIOC->PIO_PDSR, 20)
SWITCH_CASE(SW_RUD, PIOA->PIO_PDSR, 15)
SWITCH_CASE(SW_ELE, PIOC->PIO_PDSR, 31)
SWITCH_CASE(SW_AIL, PIOA->PIO_PDSR, 2)
SWITCH_CASE(SW_GEA, PIOC->PIO_PDSR, 16)
SWITCH_CASE(SW_TRN, PIOC->PIO_PDSR, 8)
#elif defined(PCBGRUVIN9X)
SWITCH_CASE(SW_THR, ping, INP_G_ThrCt)
SWITCH_CASE(SW_RUD, ping, INP_G_RuddDR)
SWITCH_CASE(SW_ELE, pinc, INP_C_ElevDR)
case DSW(SW_ID0):
ping |= (1<<INP_G_ID1);
pinb &= ~(1<<INP_B_ID2);
break;
case DSW(SW_ID1):
ping &= ~(1<<INP_G_ID1);
pinb &= ~(1<<INP_B_ID2);
break;
case DSW(SW_ID2):
ping &= ~(1<<INP_G_ID1);
pinb |= (1<<INP_B_ID2);
break;
SWITCH_CASE(SW_AIL, pinc, INP_C_AileDR)
SWITCH_CASE(SW_GEA, ping, INP_G_Gear)
SWITCH_CASE(SW_TRN, pinb, INP_B_Trainer)
#else // STOCK
#if defined(JETI) || defined(FRSKY) || defined(NMEA) || defined(ARDUPILOT)
SWITCH_CASE(SW_THR, pinc, INP_C_ThrCt)
SWITCH_CASE(SW_AIL, pinc, INP_C_AileDR)
#else
SWITCH_CASE(SW_THR, pine, INP_E_ThrCt)
SWITCH_CASE(SW_AIL, pine, INP_E_AileDR)
#endif
case DSW(SW_ID0):
ping |= (1<<INP_G_ID1);
pine &= ~(1<<INP_E_ID2);
break;
case DSW(SW_ID1):
ping &= ~(1<<INP_G_ID1);
pine &= ~(1<<INP_E_ID2);
break;
case DSW(SW_ID2):
ping &= ~(1<<INP_G_ID1);
pine |= (1<<INP_E_ID2);
#endif
break;
SWITCH_CASE(SW_RUD, ping, INP_G_RuddDR)
SWITCH_CASE(SW_ELE, pine, INP_E_ElevDR)
SWITCH_CASE(SW_GEA, pine, INP_E_Gear)
SWITCH_CASE(SW_TRN, pine, INP_E_Trainer)
#endif
default:
break;
}
src/simu.cpp
+51 -74
View File
@@ -362,86 +362,59 @@ long Open9xSim::onTimeout(FXObject*,FXSelector,void*)
}
}
#if defined(PCBSKY9X)
struct SwitchKey {
FXuint key;
volatile uint32_t & pin;
uint32_t shift;
uint32_t value;
};
#else
struct SwitchKey {
FXuint key;
volatile unsigned char& pin;
unsigned char shift;
unsigned char value;
};
#endif
static SwitchKey keys3[] = {
#if defined(PCBX9D)
{ KEY_A, PIOC->PIO_PDSR, 31, 0 },
{ KEY_B, PIOC->PIO_PDSR, 27, 0 },
{ KEY_C, PIOC->PIO_PDSR, 23, 0 },
{ KEY_D, PIOC->PIO_PDSR, 17, 0 },
{ KEY_E, PIOC->PIO_PDSR, 16, 0 },
{ KEY_F, PIOC->PIO_PDSR, 15, 0 },
{ KEY_G, PIOC->PIO_PDSR, 15, 0 },
{ KEY_H, PIOC->PIO_PDSR, 15, 0 } };
#elif defined(PCBSKY9X)
{ KEY_1, PIOC->PIO_PDSR, 20, 0 },
{ KEY_6, PIOA->PIO_PDSR, 2, 0 },
{ KEY_2, PIOA->PIO_PDSR, 15, 0 },
{ KEY_3, PIOC->PIO_PDSR, 31, 0 },
{ KEY_7, PIOC->PIO_PDSR, 16, 0 },
{ KEY_8, PIOC->PIO_PDSR, 8, 0 } };
#define SWITCH_KEY(key, swtch, states) \
static bool state##key = 0; \
static uint8_t state##swtch = 0; \
if (getApp()->getKeyState(KEY_##key)) { \
if (!state##key) { \
state##swtch = (state##swtch+1) % states; \
setSwitch(DSW(SW_##swtch+state##swtch)); \
state##key = true; \
} \
} \
else { \
state##key = false; \
}
#else
#if defined(PCBGRUVIN9X) || defined(JETI) || defined(FRSKY) || defined(NMEA) || defined(ARDUPILOT)
{ KEY_1, pinc, INP_C_ThrCt, 0 },
{ KEY_6, pinc, INP_C_AileDR, 0 },
#else
{ KEY_1, pine, INP_E_ThrCt, 0 },
{ KEY_6, pine, INP_E_AileDR, 0 },
#endif
{ KEY_2, ping, INP_G_RuddDR, 0 },
{ KEY_3, pine, INP_E_ElevDR, 0 },
//KEY_4, ping, INP_G_ID1, 0,
//KEY_5, pine, INP_E_ID2, 0,
{ KEY_7, pine, INP_E_Gear, 0 },
{ KEY_8, pine, INP_E_Trainer, 0 } };
#define SWITCH_KEY(key, swtch, states) \
static bool state##key = 0; \
static uint8_t state##swtch = 0; \
if (getApp()->getKeyState(KEY_##key)) { \
if (!state##key) { \
state##swtch = (state##swtch+1) % states; \
if (states > 2) \
setSwitch(DSW(SW_##swtch+state##swtch)); \
else \
setSwitch(state##swtch ? DSW(SW_##swtch) : -DSW(SW_##swtch)); \
state##key = true; \
} \
} \
else { \
state##key = false; \
}
#endif
for(unsigned i=0; i<DIM(keys3); i++){
bool ks = getApp()->getKeyState(keys3[i].key);
if (ks != keys3[i].value) {
if (ks) keys3[i].pin ^= (1<<keys3[i].shift);
keys3[i].value = ks;
}
}
// INP_G_ID1 INP_E_ID2
// id0 0 1
// id1 1 1
// id2 1 0
static FXuint id=0,k4st=0,k5st=0;
bool ks=getApp()->getKeyState(KEY_4);
if(ks != k4st){
if(ks && id>0) id--;
k4st = ks;
}
ks=getApp()->getKeyState(KEY_5);
if(ks != k5st){
if(ks && id<2) id++;
k5st = ks;
}
switch(id){
case 0: setSwitch(DSW(SW_ID0)); break;
case 1: setSwitch(DSW(SW_ID1)); break;
case 2: setSwitch(DSW(SW_ID2)); break;
}
#if defined(PCBX9D)
SWITCH_KEY(A, SA0, 2);
SWITCH_KEY(B, SB0, 3);
SWITCH_KEY(C, SC0, 3);
SWITCH_KEY(D, SD0, 3);
SWITCH_KEY(E, SE0, 3);
SWITCH_KEY(F, SF0, 3);
SWITCH_KEY(G, SG0, 3);
SWITCH_KEY(H, SH0, 2);
#else
SWITCH_KEY(1, THR, 2);
SWITCH_KEY(2, RUD, 2);
SWITCH_KEY(3, ELE, 2);
SWITCH_KEY(4, ID0, 3);
SWITCH_KEY(5, AIL, 2);
SWITCH_KEY(6, GEA, 2);
SWITCH_KEY(7, TRN, 2);
#endif
}
per10ms();
refreshDiplay();
getApp()->addTimeout(this,2,5);
@@ -515,6 +488,10 @@ int main(int argc,char **argv)
// so that persistent settings are now available.
application.init(argc,argv);
#if !defined(PCBX9D)
setSwitch(DSW(SW_ID0));
#endif
// This creates the main window. We pass in the title to be displayed
// above the window, and possibly some icons for when its iconified.
// The decorations determine stuff like the borders, close buttons,