From 8aea9aa3dd774322f25ef3647a5d2369ed5a8acc Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Wed, 3 Jul 2019 17:39:05 +0200 Subject: [PATCH] Added a common deadband conversion code Modified protocols: - GD00X applied on aileron - KF606 applied on aileron - POTENSIC applied on throttle --- Multiprotocol/Common.ino | 16 +++++++++++++++- Multiprotocol/GD00X_nrf24l01.ino | 14 ++------------ Multiprotocol/KF606_nrf24l01.ino | 6 ++++-- Multiprotocol/Multiprotocol.h | 2 +- Multiprotocol/POTENSIC_nrf24l01.ino | 3 ++- Multiprotocol/TX_Def.h | 2 ++ 6 files changed, 26 insertions(+), 17 deletions(-) diff --git a/Multiprotocol/Common.ino b/Multiprotocol/Common.ino index 4622b6b..c5525a1 100644 --- a/Multiprotocol/Common.ino +++ b/Multiprotocol/Common.ino @@ -56,6 +56,20 @@ void InitChannel() /************************/ /** Convert routines **/ /************************/ +// Convert channel 8b with limit and deadband +uint8_t convert_channel_8b_limit_deadband(uint8_t num,uint8_t min,uint8_t mid, uint8_t max, uint8_t deadband) +{ + uint16_t val=limit_channel_100(num); // 204<->1844 + uint16_t db_low=CHANNEL_MID-deadband, db_high=CHANNEL_MID+deadband; // 1024+-deadband + if(val>=db_low && val<=db_high) + return mid; + else if(val>3)+860; //value range 860<->2140 -125%<->+125% + return (((val<<2)+val)>>3)+860; //value range 860<->2140 -125%<->+125% } // Channel value 100% is converted to 10bit values 0<->1023 uint16_t convert_channel_10b(uint8_t num) diff --git a/Multiprotocol/GD00X_nrf24l01.ino b/Multiprotocol/GD00X_nrf24l01.ino index 307e089..61bef94 100644 --- a/Multiprotocol/GD00X_nrf24l01.ino +++ b/Multiprotocol/GD00X_nrf24l01.ino @@ -72,18 +72,8 @@ static void __attribute__((unused)) GD00X_send_packet() { packet[0]=convert_channel_16b_limit(THROTTLE,0,100); // 0..100 - // Deadband is needed on aileron - uint16_t aileron=limit_channel_100(AILERON); // 204<->1844 - #define GD00X_V2_DB_MIN 1024-40 - #define GD00X_V2_DB_MAX 1024+40 - if(aileron>GD00X_V2_DB_MIN && aileronGD00X_V2_DB_MAX) - packet[1]=0x1F-((aileron-GD00X_V2_DB_MAX)*(0x20)/(CHANNEL_MAX_100+1-GD00X_V2_DB_MAX)); // 1F..00 - else - packet[1]=0x3F-((aileron-CHANNEL_MIN_100)*(0x1F)/(GD00X_V2_DB_MIN-CHANNEL_MIN_100)); // 3F..21 - + // Deadband is needed on aileron, 40 gives +-6% + packet[2]=convert_channel_8b_limit_deadband(AILERON,0x3F,0x20,0x00,40); // Aileron: 3F..20..00 // Trims must be in a seperate channel for this model packet[2]=0x3F-(convert_channel_8b(CH5)>>2); // Trim: 0x3F..0x20..0x00 diff --git a/Multiprotocol/KF606_nrf24l01.ino b/Multiprotocol/KF606_nrf24l01.ino index 142f740..f086091 100644 --- a/Multiprotocol/KF606_nrf24l01.ino +++ b/Multiprotocol/KF606_nrf24l01.ino @@ -38,8 +38,10 @@ static void __attribute__((unused)) KF606_send_packet() { packet[0]= 0x55; packet[1]= convert_channel_8b(THROTTLE); // 0..255 - packet[2]= convert_channel_16b_limit(AILERON,0x20,0xE0); // Low:50..80..AF High:3E..80..C1 - packet[3]= convert_channel_16b_limit(CH5,0xC1,0xDF); // Trim on a separated channel C1..D0..DF + // Deadband is needed on aileron, 40 gives +-6% + packet[2]=convert_channel_8b_limit_deadband(AILERON,0x20,0x80,0xE0,40); // Aileron: Max values:20..80..E0, Low rates:50..80..AF, High rates:3E..80..C1 + // Aileron trim must be on a separated channel C1..D0..DF + packet[3]= convert_channel_16b_limit(CH5,0xC1,0xDF); } if(IS_BIND_DONE) { diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index e168526..e7fccdf 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 2 #define VERSION_REVISION 1 -#define VERSION_PATCH_LEVEL 64 +#define VERSION_PATCH_LEVEL 65 //****************** // Protocols diff --git a/Multiprotocol/POTENSIC_nrf24l01.ino b/Multiprotocol/POTENSIC_nrf24l01.ino index 88895de..5851887 100644 --- a/Multiprotocol/POTENSIC_nrf24l01.ino +++ b/Multiprotocol/POTENSIC_nrf24l01.ino @@ -44,7 +44,8 @@ static void __attribute__((unused)) POTENSIC_send_packet() else { packet[0] = 0x64; - packet[1] = convert_channel_16b_limit(THROTTLE,0,100)&0xFE; + // Deadband is needed on throttle to emulate the spring to neutral otherwise the quad behaves weirdly, 160 gives +-20% + packet[1] = convert_channel_8b_limit_deadband(THROTTLE,0x00,0x19,0x32,160)<<1; // Throttle 00..19..32 *2 uint8_t elevator=convert_channel_8b(ELEVATOR)>>3; packet[2] = ((255-convert_channel_8b(RUDDER))&0xF8)|(elevator>>2); packet[3] = (elevator<<6)|(((255-convert_channel_8b(AILERON))>>2)&0xFE); diff --git a/Multiprotocol/TX_Def.h b/Multiprotocol/TX_Def.h index e122af4..91c49a8 100644 --- a/Multiprotocol/TX_Def.h +++ b/Multiprotocol/TX_Def.h @@ -40,6 +40,8 @@ #define CHANNEL_MAX_125 2047 // 125% #define CHANNEL_MIN_125 0 // 125% +#define CHANNEL_MID 1024 + #define CHANNEL_MIN_COMMAND 784 // 1350us #define CHANNEL_SWITCH 1104 // 1550us #define CHANNEL_MAX_COMMAND 1424 // 1750us