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

Multi improvements (#5322)

Browse Source 
* implementing syncing with multi

* Send config command

* Implement setting failsafe for multimodule

* Fix

* Fix last bugs. Multi sync now working as expected.

* Add eww multi protocol
This commit is contained in:
Arne Schwabe
2017-11-11 08:57:04 +01:00
committed by Bertrand Songis
parent 8e9d370770
commit 613b2edf49
24 changed files with 409 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/pulses/multi_arm.cpp
+114 -31
View File
@@ -33,20 +33,126 @@
#define MULTI_CHANS 16
#define MULTI_CHAN_BITS 11
void sendFrameProtocolHeader(uint8_t port);
void sendChannels(uint8_t port);
static void sendSetupFrame()
{
// Old multi firmware will mark config messsages as invalid frame and throw them away
sendByteSbus('M');
sendByteSbus('P');
sendByteSbus(0x80); // Module Configuration
sendByteSbus(1); // 1 byte data
uint8_t config = 0x1 | 0x2; // inversion + multi_telemetry
#if !defined(PPM_PIN_SERIAL)
config |= 0x04; //input synchronsisation
#endif
sendByteSbus(config);
}
static void sendFailFailsafeHeader(uint8_t port)
{
// Old multi firmware will mark config messsages as invalid frame and throw them away
sendByteSbus('M');
sendByteSbus('P');
sendByteSbus(0x81); // Failsafe Data
sendByteSbus(23); // 22 byte channel + 1 byte mode
sendByteSbus(g_model.moduleData->failsafeMode);
}
static void sendFailsafeChannels(uint8_t port)
{
uint32_t bits = 0;
uint8_t bitsavailable = 0;
for (int i = 0; i < MULTI_CHANS; i++) {
int16_t failsafeValue = g_model.moduleData[port].failsafeChannels[i];
int pulseValue;
if (failsafeValue == FAILSAFE_CHANNEL_HOLD) {
pulseValue = 0;
}
else if (failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
pulseValue = 2048;
}
else {
failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[port].channelsStart + i) - 2 * PPM_CENTER;
pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2047);
}
bits |= pulseValue << bitsavailable;
bitsavailable += MULTI_CHAN_BITS;
while (bitsavailable >= 8) {
sendByteSbus((uint8_t) (bits & 0xff));
bits >>= 8;
bitsavailable -= 8;
}
}
}
void setupPulsesMultimodule(uint8_t port)
{
static int counter=0;
#if defined(PPM_PIN_SERIAL)
modulePulsesData[EXTERNAL_MODULE].dsm2.serialByte = 0 ;
modulePulsesData[EXTERNAL_MODULE].dsm2.serialBitCount = 0 ;
#else
modulePulsesData[EXTERNAL_MODULE].dsm2.rest = 18000; // 9ms refresh
modulePulsesData[EXTERNAL_MODULE].dsm2.rest = multiSyncStatus.getAdjustedRefreshRate();
modulePulsesData[EXTERNAL_MODULE].dsm2.index = 0;
#endif
modulePulsesData[EXTERNAL_MODULE].dsm2.ptr = modulePulsesData[EXTERNAL_MODULE].dsm2.pulses;
// Every 1000 cycles (=9s) send a config packet that configures the multimodule (inversion, telemetry type)
counter++;
if (counter % 1000== 500) {
sendSetupFrame();
} else if (counter % 1000 == 0) {
sendFailFailsafeHeader(port);
sendFailsafeChannels(port);
} else {
// Normal Frame
sendFrameProtocolHeader(port);
sendChannels(port);
}
// byte 1+2, protocol information
putDsm2Flush();
}
void sendChannels(uint8_t port)
{
uint32_t bits = 0;
uint8_t bitsavailable = 0;
// byte 4-25, channels 0..2047
// Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
// Multi uses [204;1843] as [-100%;100%]
for (int i = 0; i < MULTI_CHANS; i++) {
int channel = g_model.moduleData[port].channelsStart + i;
int value = channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
// Scale to 80%
value = value * 800 / 1000 + 1024;
value = limit(0, value, 2047);
bits |= value << bitsavailable;
bitsavailable += MULTI_CHAN_BITS;
while (bitsavailable >= 8) {
sendByteSbus((uint8_t) (bits & 0xff));
bits >>= 8;
bitsavailable -= 8;
}
}
}
void sendFrameProtocolHeader(uint8_t port)
{// byte 1+2, protocol information
// Our enumeration starts at 0
int type = g_model.moduleData[port].getMultiProtocol(false) + 1;
@@ -60,10 +166,10 @@ void setupPulsesMultimodule(uint8_t port)
protoByte |= MULTI_SEND_RANGECHECK;
// rfProtocol
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_DSM2){
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_DSM2) {
// Autobinding should always be done in DSMX 11ms
if(g_model.moduleData[port].multi.autoBindMode && moduleFlag[port] == MODULE_BIND)
if (g_model.moduleData[port].multi.autoBindMode && moduleFlag[port] == MODULE_BIND)
subtype = MM_RF_DSM2_SUBTYPE_AUTO;
// Multi module in DSM mode wants the number of channels to be used as option value
@@ -78,10 +184,10 @@ void setupPulsesMultimodule(uint8_t port)
// 25 is again a FrSky protocol (FrskyV) so shift again
if (type >= 25)
type = type + 1;
type = type + 1;
if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_PROTO_FRSKY) {
if(subtype == MM_RF_FRSKY_SUBTYPE_D8) {
if (subtype == MM_RF_FRSKY_SUBTYPE_D8) {
//D8
type = 3;
subtype = 0;
@@ -121,7 +227,7 @@ void setupPulsesMultimodule(uint8_t port)
// protocol byte
protoByte |= (type & 0x1f);
if(g_model.moduleData[port].getMultiProtocol(true) != MM_RF_PROTO_DSM2)
if (g_model.moduleData[port].getMultiProtocol(true) != MM_RF_PROTO_DSM2)
protoByte |= (g_model.moduleData[port].multi.autoBindMode << 6);
sendByteSbus(protoByte);
@@ -130,31 +236,8 @@ void setupPulsesMultimodule(uint8_t port)
sendByteSbus((uint8_t) ((g_model.header.modelId[port] & 0x0f)
| ((subtype & 0x7) << 4)
| (g_model.moduleData[port].multi.lowPowerMode << 7))
);
);
// byte 3
sendByteSbus((uint8_t) optionValue);
uint32_t bits = 0;
uint8_t bitsavailable = 0;
// byte 4-25, channels 0..2047
// Range for pulses (channelsOutputs) is [-1024:+1024] for [-100%;100%]
// Multi uses [204;1843] as [-100%;100%]
for (int i=0; i<MULTI_CHANS; i++) {
int channel = g_model.moduleData[port].channelsStart+i;
int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
// Scale to 80%
value = value*800/1000 + 1024;
bits |= limit(0, value, 2047) << bitsavailable;
bitsavailable += MULTI_CHAN_BITS;
while (bitsavailable >= 8) {
sendByteSbus((uint8_t) (bits & 0xff));
bits >>= 8;
bitsavailable -= 8;
}
}
putDsm2Flush();
}