From 9570f8595ae34c60df0b37c2c4a5ee508514c3f5 Mon Sep 17 00:00:00 2001 From: rotorman Date: Mon, 20 Sep 2021 10:57:07 +0200 Subject: [PATCH] Removed FlySky digital hall gimbal double value conversion (first using the FlySky factory calibration values min,mid and max to convert to 0 to 4095 and second to convert this value again into EdgeTX internal 11-bit range 0 to 2047). This has the added benefit of precision increase and also avoiding unnecessary computation. Needed to change to uint32_t, as the jitter removal computation would otherwise go over the 16-bit range. As 16-bit computation can be left for "normal" sticks, added a special define just for FLYSKY_HALL_STICKS. In addition this PR changes back the radio analogs diagnose page second column also for FlySky digital hall sticks to show +/- 100 again, as now with first and second column we already cover all internal ranges used. Tested on RM TX16S with FlySky digital hall sticks. --- radio/src/gui/colorlcd/radio_diaganas.cpp | 2 +- radio/src/opentx.cpp | 13 ++- radio/src/opentx.h | 4 + .../targets/horus/flyskyHallStick_driver.cpp | 88 ++----------------- .../targets/horus/flyskyHallStick_driver.h | 28 +----- 5 files changed, 25 insertions(+), 110 deletions(-) diff --git a/radio/src/gui/colorlcd/radio_diaganas.cpp b/radio/src/gui/colorlcd/radio_diaganas.cpp index 011313881..8bc5da3b2 100644 --- a/radio/src/gui/colorlcd/radio_diaganas.cpp +++ b/radio/src/gui/colorlcd/radio_diaganas.cpp @@ -59,7 +59,7 @@ class RadioAnalogsDiagsWindow: public Window { dc->drawNumber(x, y, i + 1, LEADING0 | LEFT, 2); dc->drawText(x + 2 * 15 - 2, y, ":"); dc->drawNumber(x + 3 * 15 - 1, y, hall_raw_values[i], LEFT); - dc->drawNumber(x + ANA_OFFSET, y, hall_adc_values[i], RIGHT); + dc->drawNumber(x + ANA_OFFSET, y, (int16_t) calibratedAnalogs[CONVERT_MODE(i)] * 25 / 256, RIGHT); } for (uint8_t i = FLYSKY_HALL_CHANNEL_COUNT; i < NUM_STICKS + NUM_POTS + NUM_SLIDERS; i++) { diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 20881ea6d..f20fc27c6 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1020,8 +1020,12 @@ void checkTrims() } #if !defined(SIMU) +#if defined(FLYSKY_HALL_STICKS) +uint32_t s_anaFilt[NUM_ANALOGS]; +#else uint16_t s_anaFilt[NUM_ANALOGS]; #endif +#endif #if defined(JITTER_MEASURE) JitterMeter rawJitter[NUM_ANALOGS]; @@ -1064,9 +1068,8 @@ void getADC() DEBUG_TIMER_STOP(debugTimerAdcRead); for (uint8_t x=0; x> (1 - ANALOG_SCALE); } else { @@ -1085,6 +1088,7 @@ void getADC() #endif } #else + uint16_t v; v = getAnalogValue(x) >> (1 - ANALOG_SCALE); #endif @@ -1122,8 +1126,13 @@ void getADC() // Variables mapping: // * = v // * = s_anaFilt[x] +#if defined(FLYSKY_HALL_STICKS) + uint32_t previous = s_anaFilt[x] / JITTER_ALPHA; + uint32_t diff = (v > previous) ? (v - previous) : (previous - v); +#else uint16_t previous = s_anaFilt[x] / JITTER_ALPHA; uint16_t diff = (v > previous) ? (v - previous) : (previous - v); +#endif if (!g_eeGeneral.jitterFilter && diff < (10*ANALOG_MULTIPLIER)) { // g_eeGeneral.jitterFilter is inverted, 0 - active // apply jitter filter s_anaFilt[x] = (s_anaFilt[x] - previous) + v; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 2d1d1ac30..f20aecfe2 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1280,8 +1280,12 @@ extern Clipboard clipboard; #endif #if !defined(SIMU) +#if defined(FLYSKY_HALL_STICKS) +extern uint32_t s_anaFilt[NUM_ANALOGS]; +#else extern uint16_t s_anaFilt[NUM_ANALOGS]; #endif +#endif #if defined(JITTER_MEASURE) extern JitterMeter rawJitter[NUM_ANALOGS]; diff --git a/radio/src/targets/horus/flyskyHallStick_driver.cpp b/radio/src/targets/horus/flyskyHallStick_driver.cpp index 36240c3a1..eb74a2ef8 100644 --- a/radio/src/targets/horus/flyskyHallStick_driver.cpp +++ b/radio/src/targets/horus/flyskyHallStick_driver.cpp @@ -29,7 +29,6 @@ unsigned char HallCmd[264] __DMA; STRUCT_HALL HallProtocol = { 0 }; STRUCT_HALL HallProtocolTx = { 0 }; signed short hall_raw_values[FLYSKY_HALL_CHANNEL_COUNT]; -STRUCT_STICK_CALIBRATION hall_calibration[FLYSKY_HALL_CHANNEL_COUNT] = { {0, 0, 0} }; unsigned short hall_adc_values[FLYSKY_HALL_CHANNEL_COUNT]; /* crc16 implementation according to CCITT standards */ @@ -93,11 +92,11 @@ uint16_t get_flysky_hall_adc_value(uint8_t ch) #if defined(FLYSKY_HALL_STICKS_REVERSE) ch = sticks_mapping[ch]; - return MAX_ADC_CHANNEL_VALUE - hall_adc_values[ch]; + return 2*FLYSKY_OFFSET_VALUE - hall_adc_values[ch]; #else if (ch < 2) { - return MAX_ADC_CHANNEL_VALUE - hall_adc_values[ch]; + return 2*FLYSKY_OFFSET_VALUE - hall_adc_values[ch]; } return hall_adc_values[ch]; @@ -153,7 +152,6 @@ uint8_t HallGetByte(uint8_t * byte) return hallDMAFifo.pop(*byte); } - void reset_hall_stick( void ) { unsigned short crc16 = 0xffff; @@ -171,23 +169,6 @@ void reset_hall_stick( void ) HallSendBuffer( HallCmd, 6); } -void get_hall_config( void ) -{ - unsigned short crc16 = 0xffff; - - HallCmd[0] = FLYSKY_HALL_PROTOLO_HEAD; - HallCmd[1] = 0xD1; - HallCmd[2] = 0x01; - HallCmd[3] = 0x00; - - crc16 = calc_crc16(HallCmd, 4); // 2B 2C - - HallCmd[4] = crc16 & 0xff; - HallCmd[5] = crc16 >>8 & 0xff ; - - HallSendBuffer( HallCmd, 6); -} - void flysky_hall_stick_init() { NVIC_InitTypeDef NVIC_InitStructure; @@ -325,45 +306,9 @@ exit: return; } -void convert_hall_to_adc_value( void ) -{ - uint16_t value; - - for ( uint8_t channel = 0; channel < 4; channel++ ) - { - if (hall_raw_values[channel] < hall_calibration[channel].mid) - { - value = hall_calibration[channel].mid - (hall_calibration[channel].min+FLYSKY_HALL_ERROR_OFFSET); - value = ( MIDDLE_ADC_CHANNLE_VALUE * (hall_calibration[channel].mid - hall_raw_values[channel] ) ) / ( value ); - - if (value >= MIDDLE_ADC_CHANNLE_VALUE ) { - value = MIDDLE_ADC_CHANNLE_VALUE; - } - - hall_adc_values[channel] = MIDDLE_ADC_CHANNLE_VALUE - value; - } - else - { - value = (hall_calibration[channel].max - FLYSKY_HALL_ERROR_OFFSET) - hall_calibration[channel].mid; - - value = ( MIDDLE_ADC_CHANNLE_VALUE * (hall_raw_values[channel] - hall_calibration[channel].mid ) ) / (value ); - - if (value >= MIDDLE_ADC_CHANNLE_VALUE ) - { - value = MIDDLE_ADC_CHANNLE_VALUE; - } - - hall_adc_values[channel] = MIDDLE_ADC_CHANNLE_VALUE + value + 1; - } - } -} - /* Run it in 1ms timer routine */ void flysky_hall_stick_loop(void) { - static tmr10ms_t lastConfigTime = get_tmr10ms(); - bool log = 0; - uint8_t byte; while(HallGetByte(&byte)) @@ -379,34 +324,15 @@ void flysky_hall_stick_loop(void) switch ( HallProtocol.hallID.hall_Id.receiverID ) { case TRANSFER_DIR_TXMCU: - if(HallProtocol.hallID.hall_Id.packetID == FLYSKY_HALL_RESP_TYPE_CALIB) { - memcpy(&hall_calibration, HallProtocol.data, sizeof(hall_calibration)); - } - else if(HallProtocol.hallID.hall_Id.packetID == FLYSKY_HALL_RESP_TYPE_VALUES) { + if(HallProtocol.hallID.hall_Id.packetID == FLYSKY_HALL_RESP_TYPE_VALUES) { memcpy(hall_raw_values, HallProtocol.data, sizeof(hall_raw_values)); - convert_hall_to_adc_value(); + for ( uint8_t channel = 0; channel < 4; channel++ ) + { + hall_adc_values[channel] = FLYSKY_OFFSET_VALUE + hall_raw_values[channel]; + } } break; } } } - //check periodically if calibration is correct - if (get_tmr10ms() - lastConfigTime > 200) - { - //invalid calibration - if(hall_calibration[0].max - hall_calibration[0].min < 1024) { - TRACE("GET HALL CONFIG"); - get_hall_config(); - lastConfigTime = get_tmr10ms(); - } - - if (log) - { - for (int idx = 0; idx < HallProtocol.length + 5; idx++) - { - TRACE_NOCRLF(" %02X", *((uint8_t*)&HallProtocol + idx)); - } - TRACE(";"); - } - } } diff --git a/radio/src/targets/horus/flyskyHallStick_driver.h b/radio/src/targets/horus/flyskyHallStick_driver.h index af9b1bd24..8d7973990 100644 --- a/radio/src/targets/horus/flyskyHallStick_driver.h +++ b/radio/src/targets/horus/flyskyHallStick_driver.h @@ -23,28 +23,10 @@ #define FLYSKY_HALL_BAUDRATE ( 921600 ) #define FLYSKY_HALL_CHANNEL_COUNT ( 4 ) -#define MAX_ADC_CHANNEL_VALUE ( 4095 ) -#define MIN_ADC_CHANNLE_VALUE ( 0 ) -#define MIDDLE_ADC_CHANNLE_VALUE ( 2047 ) +#define FLYSKY_OFFSET_VALUE ( 16384 ) #define FLYSKY_HALL_PROTOLO_HEAD 0x55 -#define FLYSKY_HALL_RESP_TYPE_CALIB 0x0e #define FLYSKY_HALL_RESP_TYPE_VALUES 0x0c -#define FLYSKY_HALL_ERROR_OFFSET 10 - -typedef struct -{ - signed short min; - signed short mid; - signed short max; -} STRUCT_STICK_CALIBRATION; - -typedef struct -{ - STRUCT_STICK_CALIBRATION sticksCalibration[4]; - unsigned char reststate; - unsigned short CRC16; -} STRUCT_STICK_CALIBRATION_PACK; typedef struct { @@ -53,12 +35,6 @@ typedef struct unsigned short CRC16; } STRUCT_CHANNEL_PACK; -typedef union -{ - STRUCT_STICK_CALIBRATION_PACK channelPack; - STRUCT_CHANNEL_PACK sticksCalibrationPack; -} UNION_DATA; - typedef struct { unsigned char start; @@ -66,7 +42,7 @@ typedef struct unsigned char receiverID:2; unsigned char packetID:4; unsigned char length; - UNION_DATA payload; + STRUCT_CHANNEL_PACK payload; } STRUCT_HALLDATA; typedef struct