zurück zum PwmReceiver Bericht
/*-----------------------------------------------------------------------------------------
* This software is in the public domain, furnished "as is", without technical
* support, and with no warranty, express or implied, as to its usefulness for
* any purpose.
*
* PwmReceive.ino
*
* PWM Signal decoder for multiple channels of RC receivers. For each cannel one rmt channel
* including ISR Interrupt must be used. Therefor ammount is limited. This decoder uses
* the rmt inviroment of the ESP32 microcontroller architectur with ESP-IDF v5.x of the ESP32 Core.
*
* Following functions are included:
* - configuration of number of measuered RC pwm channels
* - calibration functions for pwm channels pulse range array
* - pwm pulse signal quality monitoring including rescue and failsafe for
* - pwm pulse range and boundary
* - pwm pulse scattering
* - pwm pulse cycle
* - support for channel invertation
* - fine trim compensation function
* fine trim area
* |<- boundary ->| |xx| |xx|
* ________________ ________________
* | | pwm | | | | | |
* | | signal | | | | | |
* ___| | range | |______________ *n ... ___| | | |______________
*
* | <-------- pwm pulse cycle (20ms) --------> | | | |pwm_max
* | |pwm_mid
* |pwm_min
*
* The pulse length for RC channels should be in range of 1.0 <-> 1.5 <-> 2.0ms (1000 to 2000us).
* This implementation calibrates the pwm pulse data to float values in the range -1.0 <-> 0 <-> 1.0.
* To collect the pulse length data from GPIO Pin the rmt and interupt functions are used.
* Therefore also 2ms of CPU time per cycle is needed to measure the pwm pulse. To minimize the
* measurement time in sequenze RC channels should be used. With e.g. 4 channels 8ms of runtime will
* be needed for receiver an only 11 to 12ms will be left for other functions.
*
* For all old RC receiver with 5 Volt a Voltage divider for each GIO is needed to connect to ESP32.
* Use a 2200Ohm resitor between RC receiver output and GIO-Pin
* and a 3300Ohm resistor between GIO-Pin and GND
*
* Author: Volker Frauenstein
* Date: 18/09/2025 Version: 1.12 configuration change in pwm_trim (fine trim deactivated for channel 2)
*
* Date: 07/07/2025 Version: 1.11 runtime reseve calculation implies now pulslenght of first channel and
* update errorhandling with disenable and re-enable rmt channel at not received pwm
* Date: 15/05/2025 Version: 1.10 runtime reseve calculation in debug_SM added
* Date: 12/05/2025 Version: 1.9 flexibel threshold definitions for rescue and RC cycle time check
* function getChAmmount added to avoid pwm_channel_count as global definition
* Date: 12/05/2025 Version: 1.8 support for definitions of functional RC inputs added with
* new function MapRCtoFunction
* Date: 11/05/2025 Version: 1.7 deactivation function for fine trim compensation per RC channel added
* function should be used for channels with Multiswitch
* Date: 10/05/2025 Version: 1.6 RC pwm receiver state machine migrated
* Date: 08/05/2025 Version: 1.5 scattering monitoring migrated
* Date: 07/05/2025 Version: 1.4 channel invertetion function added
* Date: 06/05/2025 Version: 1.3 adding supporting functions to calibrate and norm pwm signal range
* and to compensate pwm range shift due to transiver fine trim
* Date: 05/05/2025 Version: 1.2 stacking of rmt_receive calls added to catch all pwm pulses within
* on RC pwm cycle of 20ms
* Date: 04/05/2025 Version: 1.1 support for more than 1 pwm channle added, RC cycle timing added
* Date: 03/05/2025 Version: 1.0 first implementation of rmt interrupt routine for 1 RC channel
* based on the ESP-IDF API Reference V5.4.1 example implementation
*/
// Definitions to handle pulse lenght measurement with the ESP32 Remote Control Transceiver peripheral
// #include "driver/rmt_rx.h" // include transfered to project header
// definition for pinout for RC input signals *****************************************************
#define pwm_channel_count 4 // Number of PWM signal input pins
const uint8_t pwm_pins[pwm_channel_count] = {36, 39, 34, 35}; // array of input pins (numbers)
// definition for RC input signal processing functions *********************************************
/* interface definitions to handle input signals from RC - transfered to project header to handle globaly
volatile bool pwmReadDone = false; // semaphore to trigger next loop after new RC Signal read
volatile bool validPWM = false; // semaphore to trigger failsafe if false
volatile int panic = 0; // counter for RC signal cycle errorlevel
volatile int RC_cycle = 0; // monitoring value for RC signal cycle duration */
// Control input signals
uint16_t pwmSigRaw[pwm_channel_count]; // Current RC signal raw pulse width [X] = channel number
rmt_channel_handle_t pwm_channel[pwm_channel_count]; // define handle on rmt channels
float pwmRCin[pwm_channel_count]; // an array to store the calibrated input from receiver
uint16_t lastPulseGood[pwm_channel_count]; // last good RC signal RAW pulse width [X] = channel number
int lastGood [pwm_channel_count]; // counterarry for quality of last RC signal RAW pulse width
int RCpwmCount[pwm_channel_count] = {0}; // counter for RC channels with big signal deltas per loop
// definition for rc pwm receiver state maschine
bool failed_before = false; // flag to store previous RC signal quality state
bool failed_now = true; // flag to store actuel RC signal quality state
int count_good = 0; // hysteresecounter for RC signal quality measurement
// definitions for the RC Signal protection functions
uint16_t pwmLast[pwm_channel_count]; // an array to buffer last input from receiver
uint16_t pwmDelta[pwm_channel_count]; // an array to buffer delta betwin last and actual pulsewidth measurements
// definitions for RC cycle timing measurement
unsigned long lastloop = 0; // definition of time stamp last cycle
// definitions for adjustable values **************************************************************
// adjustable values for RC input pwm pulses in us (normrange 1000, 1500, 2000)
// configuration for minimal, midpont and maximal pulse length per cannel is needed in array below
// configuration for invertation of channel and if the channel has fine trim is needed as well
// old robbe FMSS systems should have a pulse range of 650, 1300, 1950 us
// Default configuration with expanded bounds for first calibration
// for old and newer Robbe systems CH1 CH2 CH3 CH4 // Robbe Terra Top FMSS/PCMS RC cycle 22/23ms
uint16_t pwm_min[pwm_channel_count]={ 600, 600, 600, 600};
uint16_t pwm_mid[pwm_channel_count]={1350, 1350, 1350, 1350};
uint16_t pwm_max[pwm_channel_count]={2100, 2100, 2100, 2100};
bool pwm_invert[pwm_channel_count] ={false, false, false, false};// array to config if channel is inverted
bool pwm_trim[pwm_channel_count] ={true, false, true, true};// array to config if channel has fine trim
// Robbe Futaba Skysport 4 with Multiswitch encoder on channel 2) 4CH 40MHz with Robbe Futaba FP-R115F receiver
/* with pulse range of 960-1520-2080us CH1 CH2 CH3 // runtime reserve all 3 CH -> 16ms at RC cycle 21/22ms
uint16_t pwm_min[pwm_channel_count] = { 1067, 924, 1181 };// CH1-4: runtime reserve 15ms, CH1,2,3+5 runtime reserve 14ms
uint16_t pwm_mid[pwm_channel_count] = { 1531, 1522, 1505 };// CH1,2,3+6: runtime reserve 13ms, CH1,2,3+7 runtime reserve 11ms
uint16_t pwm_max[pwm_channel_count] = { 1999, 2120, 1855 };// CH1,2,3+8: runtime reserve 4 but CH8 is 1, CH1 is 2, ...
bool pwm_invert[pwm_channel_count] = { false, false, false};// array to config if channel is inverted
bool pwm_trim[pwm_channel_count] = { true, false, true };// array to config if channel has fine trim */
// Robbe Starion 4CH 40MHz with Robbe FMSS-micro receiver (7CH)
/* with pulse range 650-1300-1950us CH1 CH2 CH3 CH4 // runtime reserve all 4 CH -> 15ms at RC cycle 21/22ms
uint16_t pwm_min[pwm_channel_count]={ 818, 837, 829, 821};// runtime reserve 3 CH -> 16ms at RC cycle 21/22ms (14ms CH1,2,4)
uint16_t pwm_mid[pwm_channel_count]={1316, 1337, 1325, 1309};// runtime reserve 2 CH -> 18ms at RC cycle 21/22ms (16ms CH1,3) (15ms CH1,4)
uint16_t pwm_max[pwm_channel_count]={1815, 1837, 1822, 1806};// runtime reserve 1 CH -> 19ms at RC cycle 21/22ms
bool pwm_invert[pwm_channel_count] ={true, true, false, true};// array to config if channel is inverted
bool pwm_trim[pwm_channel_count] ={true, false, true, true};// array to config if channel has fine trim */
const int th_good = 5; // ammount of RC periods with valid pulses to go out of failsafe (run againg condition)
const int th_scat = 99; // treshold for scatering determination
const int SC_threshold = 5; // ammount of RC periods before one channel scattering forces failed (fast movement protection)
const int th_ch = 1; // treshold for number of paralle scattering channels (1 means failsafe if 2 channels fail)
const int SC_chold = 3; // ammount of RC periods before multiple channel scattering forces failed (fast movement protection)
const uint16_t pwm_bound_min = 680; // boundery definitionen for RC signal (min. value depends on RC System, check with fine trim)
const uint16_t pwm_bound_max = 1950; // boundery definitionen for RC signal (max. value depends on RC System, check with fine trim)
const int BC_threshold[pwm_channel_count] = { 5, 10, 5, 5 }; // ammount of rescued RC periods per channel if boundarycheck failed (higher trashold for Multiswitch channel)
const int RCcycleMin = 16; // minimum RC cycle time in ms without forcing a failur
const int RCcycleMax = 24; // maximum RC cycle time in ms without forcing a failur
// Global variables for included functions ********************************************************
// definitions for rmt interrupt infrastructur
#define scan_resolution 1000000 // 1MHz resolution, 1 tick = 1us
QueueHandle_t receive_queue; // define queue for pwm measurement result
rmt_symbol_word_t raw_symbols[64]; // definition of RMT symbols - 1 symbols should be sufficient for on RC pwm pulse in interval but min is 64
rmt_rx_done_event_data_t rx_data; // definition of rmt callback measurement data
rmt_rx_event_callbacks_t cbs; // define handle on rmt callback
rmt_receive_config_t receive_cfg = { // config the filter settings for rmt receiver
.signal_range_min_ns = 1250, // the shortest duration for RC PWM Channel signal is 0,65 ms -> lot longer than 1250ns
.signal_range_max_ns = 2200000, // the longest duration for RC PWM Channel signal is 2,2ms => 2200000 ns - the receive won't stop early
};
// generig definitions for debug - uncomment it if rmt debugging or RC signal calibration debuging is needed
//#define DEBUG_RMT 1 // debuging flag for rmt ISR functions and rmt input signal
//#define DEBUG_SM 1 // debuging flag for RC signal state machine and runtime reseve measurement
//#define CHANNEL_DEBUG 1 // debuging flag for RC signal output
//#define DEBUG_SCAT 1 // debuging flag for RC signal scattering
#define CHANNEL_CALIB 1 // debuging flag for RC signal calibration
#ifdef DEBUG_SM // timestamp definitions for runtime reserve calculation
unsigned long StartRead; // start time of runPwmReciever
unsigned long StartCollect; // first callback in cycle
int minReserve = 25; // buffer for minimal runtime reserve
#endif
// definitions for RC PWM Signal calibration function init_RCpwmRange
#ifdef CHANNEL_CALIB
uint16_t PWminA[pwm_channel_count]; // an array to buffer min value of pulsewidth measurements
uint16_t PWmaxA[pwm_channel_count]; // an array to buffer max value of pulsewidth measurements
#endif
void setup_PwmChannels(void) { // setup function for RC pwm giopins and rmt cannel configuration
rmt_rx_channel_config_t channel_cfg; // definition of rmt channel config for pwm channel scanning
cbs = { .on_recv_done = rmt_pwm_done_callback, }; // setup handle on rmt callback
// create handle for pwm measurement result
receive_queue = xQueueCreate(1, sizeof(rmt_rx_done_event_data_t));
assert(receive_queue); // establisch queue
for (int i = 0; i < pwm_channel_count; i++) { // loop all needed giopins and rmt cannels to config
pinMode(pwm_pins[i], INPUT_PULLDOWN); // gio pinmode is pulldown
rmt_rx_channel_config_t channel_cfg = { // config for PWM signal -> channel i
.gpio_num = (gpio_num_t)pwm_pins[i], // dedicated gio pin of pwm signal
.clk_src = RMT_CLK_SRC_DEFAULT, // 80Mhz for the group is default
.resolution_hz = scan_resolution, // resolution of the scanning on the channel
.mem_block_symbols = 64, // amount of RMT symbols that the channel can store at a time (min is 64)
.intr_priority = 0, // interupt priority low
.flags = {
.invert_in = false, // do not invert input signal
.with_dma = false, // do not use DMA backend
.io_loop_back = false, // do not activate for debug/test
.allow_pd = false, // do not allow sleep mode
}
};
// create rmt channel for pwm channel scanning or printout error
// gpio_pullup_en(78): GPIO number error with GPIO 34,36 and 39
ESP_ERROR_CHECK(rmt_new_rx_channel(&channel_cfg, &pwm_channel[i]));
// enable rmt channel for pwm channel scanning or printout error
ESP_ERROR_CHECK(rmt_enable(pwm_channel[i]));
#ifdef DEBUG_RMT
gpio_dump_io_configuration(stdout, (1ULL << pwm_pins[i]));
#endif
}
#ifdef DEBUG_RMT
Serial.println("Config of giopins and rmt channels finished");
#endif
#ifdef CHANNEL_CALIB // setup for channel calibration if needed
CheckCentre();
init_RCpwmRange();
#endif
} // end of function setup_pwm_channels
bool runPwmReciever (void) { // function returns RC receiver state machine status
// get all RC input values for this loop
read_PwmChannels(); // receive all pwm channel signals for this cycle
if (pwmReadDone) { // wait till pwm data read hase been finisched
pwmReadDone = false; // cycle ongoing, prepaire for next
if (!failed_now) { // old RC systems can have scattering on pwm signal, therefor the out of bound check will not do the job
failed_now = RCpwmScattering(); // all channels are inbound, check RC signal scattering
} // no else , we are in error failure state
#ifdef DEBUG_SM
Serial.print("RC State: ");
#endif
// check if we have a reasonable RC signal cycle (should be 20ms)
if ((RC_cycle > RCcycleMin) && (RC_cycle < RCcycleMax)) {
if ( failed_before ) { // failsafe is activ
if ( !failed_now ) { // check if signal quality is now good againg
if ( count_good > th_good ) { // signal was threshold times good
failed_before = false; // we can clean up for next check
count_good = 0;
panic = 0;
validPWM = false; // system in failsafe for this cycle
#ifdef DEBUG_SM
Serial.print(" Start ");
#endif
}
else { // startup or restart situation ongoing
count_good = count_good + 1; // signal is good -> count good time
validPWM = false; // system in failsafe for this cycle
if (panic > 0) {panic--;} // decrease failure counter if needed
#ifdef DEBUG_SM
Serial.print(" Wait ");
#endif
}
}
else { // signal is still or againg bad
count_good = 0; // reset good time counter
validPWM = false; // system in failsafe for this cycle
#ifdef DEBUG_SM
Serial.print(" Reset ");
#endif
}
}
else { // RC signal was good, check if it is still good
if ( !failed_now ) { // yes -> normal run situation -> place function call below
count_good = 0;
validPWM = true; // system in run for this cycle, signals can be used
#ifdef DEBUG_SM
Serial.print(" Run ");
Serial.print("[");Serial.print(pwmRCin[0]);
for (int i = 1; i < pwm_channel_count; i++) {
Serial.print(",");Serial.print(pwmRCin[i]);
}
Serial.print("]");
int calcReserve = StartCollect-StartRead-2; // calculated runtime reserve -2 for first pwm pulse, rmt setup and signal quality checks
if (minReserve > calcReserve) {minReserve = calcReserve;} // find minimum
#endif
}
else { // RC signal has gone bad now, do failsafe and disable functions
failed_before = true; // prepare for next check
count_good = 0;
validPWM = false; // system in failsafe for this cycle
#ifdef DEBUG_SM
Serial.print(" Stop ");
#endif
}
} // end failsafe check for old RCs
} // end good RC pwm signal cycle
else { // no RC signal, keep failsafe and disable RC functions
if (panic < 10) { // wait for 10 negativ cycles
failed_before = true; // prepare for next check
count_good = 0;
validPWM = false; // system in failsafe for this cycle
panic++; // count up RC failure level
#ifdef DEBUG_SM
Serial.print(" NO Cycletime ");
#endif
}
else { // no RC signal for long time do nothing any more
#ifdef DEBUG_SM
Serial.print(" RC Lost ");
#endif
}
} // end RC data check
#ifdef DEBUG_SM // show debug data if needed
int pwmC1 = round(pwmSigRaw[0]/1000); // get measured time of first channel pwm pulse lenght in ms
Serial.print(", last RC cycle: ");Serial.print(RC_cycle);Serial.print("ms");
Serial.print(", run reseve: ");Serial.print(StartCollect-StartRead-pwmC1);Serial.println("ms");
//Serial.print(", RC failure: ");Serial.println(panic);
#endif
if (!validPWM) { MapRCtoFailsafe(); } // set internal RC signal to failsafe position if needed
return validPWM; // return tranceiver state maschine status
} // end pwmReadDone
} // end of function runPwmReciever
void read_PwmChannels(void) { // function to set up rmt callbacks and read out pwm pulses
// rmt - order all needed isr and rmt resources for this RC cycle
for (int i = 0; i < pwm_channel_count; i++) { // loop all needed cannels to activate callback and receive
// create callback for rmt channel for continues measurement of pwm pin
ESP_ERROR_CHECK(rmt_rx_register_event_callbacks(pwm_channel[i], &cbs, receive_queue));
// and start receiver for collection
ESP_ERROR_CHECK(rmt_receive(pwm_channel[i], raw_symbols, sizeof(raw_symbols), &receive_cfg));
}
#ifdef DEBUG_SM // interupts are established for this cycle
StartRead = millis(); // wait period and runtime reserve starts now
#endif
// wait and collect - for isr and rmt interupt callback trigger and get measuremt results buffer
for (int i = 0; i < pwm_channel_count; i++) { // loop to collect all PWM duration data from ISR
// wait for RX done signals
if (xQueueReceive(receive_queue, &rx_data, pdMS_TO_TICKS(100)) == pdPASS) {
#ifdef DEBUG_SM
if (i==0) { // wait time is over
StartCollect = millis(); // get time stamp
}
#endif
// parse the receive symbols and store result in array
pwmSigRaw[i] = parse_pwm_duration(rx_data.received_symbols, rx_data.num_symbols);
}
else {
ESP_ERROR_CHECK(rmt_disable(pwm_channel[i])); // errorhandling: disenable rmt channel
Serial.print("No RX on channel: ");Serial.println(i+1); // communicate error and give rmt time to close
ESP_ERROR_CHECK(rmt_enable(pwm_channel[i])); // and enable rmt channel againg
}
}
pwmReadDone = true; // read job done for this pwm cycle
failed_now = false; // set flag for good pulse length, failure will change status
unsigned long loopnow = millis(); // get timestamp for pwm cycle measurement
RC_cycle = loopnow - lastloop; // calc RC signal duration
lastloop = loopnow; // store this timestamp for next calculation
#ifdef DEBUG_RMT
// print pwm pulse duration and RC cycle time if needed
Serial.print("row pwm pulse length us: ["); Serial.print(pwmSigRaw[0]);
for (int i = 1; i < pwm_channel_count; i++) {
Serial.print(",");Serial.print(pwmSigRaw[i]);
}
Serial.print("] -> Duration: ");Serial.print(RC_cycle);Serial.println("ms");
#endif
// validate - all pwm pulse data is read, now normalize measurement
for (int i = 0; i < pwm_channel_count; i++) {
#ifdef CHANNEL_CALIB // print signal min, max range for calibration
print_RCpwmRange(i);
#endif
pwmRCin[i] = NormSignal(i); // decode receiver channel and nomalize them
}
#ifdef CHANNEL_DEBUG // print normalised RC input values for debug
// print normalised pwm pulse
Serial.print("pwmRCin: ["); Serial.print(pwmRCin[0]);
for (int i = 1; i < pwm_channel_count; i++) {
Serial.print(",");Serial.print(pwmRCin[i]);
}
Serial.print("] -> RC Signal: ");
if (!failed_now) {Serial.println("valid");}
else {Serial.println("invalid");}
#endif
} // end of function read_pwm_channels
// function to parse pwm pulse duration from ISR buffer for one channel
uint16_t parse_pwm_duration (rmt_symbol_word_t *rmt_pwm_symbols, size_t symbol_num) {
if (rmt_pwm_symbols[0].level0 > 0) { // check if pulse was high
return rmt_pwm_symbols[0].duration0; // collect first puls duration
}
else {
return 0; // bad signal
}
} // end of function rmt_pwm_done_callback
// ISR collection queue on callback
static bool rmt_pwm_done_callback(rmt_channel_handle_t channel, const rmt_rx_done_event_data_t *edata, void *user_data) {
BaseType_t high_task_wakeup = pdFALSE;
QueueHandle_t receive_queue = (QueueHandle_t)user_data;
// send the received RMT symbols to the parser task
xQueueSendFromISR(receive_queue, edata, &high_task_wakeup);
return high_task_wakeup == pdTRUE;
} // end of function rmt_pwm_done_callback
// function to check pwm raw pulse signal is in range, including invertation and fine trim compensation
float NormSignal(int i) { // function to calculate and invert normed values for pwmRCin
// check if RC cannel number is out of range, should never happen
if(i < 0 || i > pwm_channel_count) {
#ifdef CHANNEL_DEBUG
Serial.print(" channel number is out of bounds: ");Serial.println(i);
#endif
failed_now = true; // set flag for bad pulse length
return 0; // if channel number is out of bounds return zero.
}
// check if signal is in range and normalize
if((pwmSigRaw[i] >= pwm_min[i]) && pwmSigRaw[i] <= (pwm_max[i])) {
int pwm_out; // definition of return value if signal is in normal range
// map pulse length to INT range -1000 to 1000
if (pwm_invert[i]){ // check if RC channel signal should be inverted
pwm_out = map(pwmSigRaw[i],pwm_min[i],pwm_max[i],1000,-1000);
}
else{ // no inversion needed
pwm_out = map(pwmSigRaw[i],pwm_min[i],pwm_max[i],-1000,1000);
}
lastPulseGood[i] = pwmSigRaw[i]; // store for rescue
lastGood[i] = 0; // set good flag back
return (float)pwm_out / 1000; // normalize to float -1 to 1 and return
}
else { // signal out of range
float pwm_sig; // definition of return value if signal is in fine trim range or rescued
// check if fine trim function of RC transmitter has been used
if ((pwm_trim[i]) && (pwmSigRaw[i] > pwm_bound_min) && (pwmSigRaw[i] < pwm_min[i])) {
// fine trim in direction lower (negativ) used and allowed
uint16_t pwm_fine = pwm_min[i] - pwmSigRaw[i]; // calc fine trim delta
pwm_min[i] = pwmSigRaw[i]; // set new min value in array
pwm_mid[i] = pwm_mid[i] - pwm_fine; // reduce mid value in array
pwm_max[i] = pwm_max[i] - pwm_fine; // reduce max value in array
pwm_sig = TrimSignal( false, pwm_invert[i], i); // normalize to float within fine trim
#ifdef CHANNEL_DEBUG // finisch printout at debug
Serial.print(pwm_fine);Serial.println(" )");
#endif
lastPulseGood[i] = pwmSigRaw[i]; // store for rescue
lastGood[i] = 0; // set good flag back
return pwm_sig; // return normed value
}
else if ((pwm_trim[i]) && (pwmSigRaw[i] < pwm_bound_max) && (pwmSigRaw[i] > pwm_max[i])) {
// fine trim in direction higher (positiv) used and allowed
uint16_t pwm_fine = pwmSigRaw[i] - pwm_max[i]; // calc fine trim delta
pwm_max[i] = pwmSigRaw[i]; // set new max value in array
pwm_mid[i] = pwm_mid[i] + pwm_fine; // increase mid value in array
pwm_min[i] = pwm_min[i] + pwm_fine; // increase min value in array
pwm_sig = TrimSignal( true, pwm_invert[i], i); // normalize to float within fine trim
#ifdef CHANNEL_DEBUG // finisch printout at debug
Serial.print(pwm_fine);Serial.println(" )");
#endif
lastPulseGood[i] = pwmSigRaw[i]; // store for rescue
lastGood[i] = 0; // set good flag back
return pwm_sig; // return normed value
} // end calculate fine trim
else { // signal out of boundery -> handle this issue
// check if rescue is possible
if (lastGood[i] < BC_threshold[i]) { // rescue if last good signal is not to long ago
Serial.print("Boundcheck warning ");
if (pwm_trim[i]) {
Serial.print(" Trim CH ");
}
else {
Serial.print(" noTrim CH ");
}
Serial.print(i);Serial.print(" (");Serial.print(pwmSigRaw[i]);Serial.print(")");
if (pwm_invert[i]) { // check if RC channel signal should be inverted
pwm_sig = map(lastPulseGood[i],pwm_min[i],pwm_max[i],1000,-1000);
}
else { // no inversion needed
pwm_sig = map(lastPulseGood[i],pwm_min[i],pwm_max[i],-1000,1000);
}
lastGood[i] ++; // count rescues
return pwm_sig/1000; // return normed value
} // end rescue positive
else { // no rescue -> threshole value has been reached
failed_now = true; // set flag for bad pulse length
if (panic < 10) {panic++;} // count up RC failure level if not already high
#ifdef DEBUG_SM
Serial.print(", panic noRescu ");
#endif
return 0; // return failsafe value
} // end rescue check
} // end signal out of boundery
} // end signal out of range
} // end of function NormSignal
float TrimSignal (bool positiv, bool inv, int i) { // function to handle actual pwmRCin value at fine trim
#ifdef CHANNEL_DEBUG // print out shifted range array values with debug on
Serial.print("FineTrim[");Serial.print(i);
if (!positiv) {
Serial.print("] - { "); // lower raw value pwm_min
}
else { // higer raw value pwm_max
Serial.print("] + { ");
}
Serial.print(pwm_min[i]);
Serial.print(" , ");Serial.print(pwm_mid[i]);Serial.print(" , ");Serial.print(pwm_max[i]);
Serial.print(" } Delta ( ");
#endif
if (!positiv) { // lower raw values for pwm_mxx[i]
if (inv) { // but channel should be inverted
return 1; // return max positiv value
}
else{ // no invertion
return -1; // return max negativ value
}
}
else { // higer raw values for pwm_mxx[i]
if (inv) { // but channel should be inverted
return -1; // return max negativ value
}
else{ // no invertion
return 1; // return max positiv value
}
}
} // end of function TrimSignal
bool RCpwmScattering(){ // calculate scattering of RC PWM signals
int scater_CH = 0; // counter for channels with scatering
for (int i = 0; i < pwm_channel_count; i++) { // check for all channels
// check scattering for pulse width per channel
if (RCpwmCount[i] < 1) {
pwmLast[i] = pwmSigRaw[i]; // first time buffer min and max values per Channel
pwmDelta[i] = 0; // start -> no delta
RCpwmCount[i] = 1; // normal running number
}
else {
pwmDelta[i] = (pwmLast[i] + pwmSigRaw[i]); // calc delta to last pwm signal of channel
pwmDelta[i] =abs(pwmDelta[i] - (2*pwmSigRaw[i]));
if (pwmDelta[i] > th_scat ) { // if to much scattering on channel-> failsafe
#ifdef DEBUG_SCAT // print out scattering information at debug
Serial.print(" Scattering CH");Serial.print(i);
Serial.print(" ( Delta: ");Serial.print(pwmDelta[i]);
#endif
RCpwmCount[i]++; // count up ammount of scatter cycles for this channel
scater_CH++; // count up number of scattering channels
if (RCpwmCount[i] > SC_threshold) { // check if number of faulty cycles is reached
#ifdef DEBUG_SCAT // print out scattering information at debug
Serial.print(", SequenzeCount: ");Serial.print(RCpwmCount[i]);Serial.println(" )");
#endif
RCpwmCount[i] = 0; // pepare for next cycle
if (panic < 10) {panic++;} // count up RC failure level if not already high
#ifdef DEBUG_SM
Serial.print(", panic SC_high ");
#endif
return true; // return failsafe status - long time scattering on one channel
}
#ifdef DEBUG_SCAT // print out scattering information at debug
Serial.println(" )");
#endif
}
else if (RCpwmCount[i] > 1) { // no scattering, belittle scatter counter
RCpwmCount[i]--;
}
}
pwmLast[i] = pwmSigRaw[i]; // buffer current pwm value as last one for next cycle
}
if (scater_CH > th_ch) { // more then 1 channel must have scatering in this cycle
int scats = 0; // counter scattering evidens in the last cycles
for (int i = 0; i < pwm_channel_count; i++) { // sum up all scatter channel counter
scats = scats + RCpwmCount[i];
}
scats = scats/pwm_channel_count; // and normalize over number of channels
if (scats >= SC_chold) { // check scattering on multiple channels for to long
if (panic < 10) {panic++;} // count up RC failure level if not already high
#ifdef DEBUG_SM
Serial.print(", panic SC_multiC ");
#endif
return true; // return failsafe status
}
}
MapRCtoFunction(); // copy pwmRCin to functional RC input variables
return false; // no failsafe relevant scattering in this cycle
} // end of function RCpwmScattering
void MapRCtoFunction() { // copy pwmRCin to functional RC inputs variables as defined in project header
RC_Rudder = pwmRCin[0]; // RC input channel 1 is for Steering normed from 1 to -1
RC_MultiSwitch = pwmRCin[1]; // RC input Channel 2 is for Multiswich normed from 1 to -1
RC_Channel3 = pwmRCin[2]; // RC input Channel 3 from 1 to -1
RC_Trottle = pwmRCin[3]; // RC input Channel 4 is for Trottle normed from 1 to -1
} // end of function MapRCtoFunction
void MapRCtoFailsafe() { // set SW minternaly RC signals to failsave
RC_Rudder = 0; // RC input channel 1 is for Steering -> failsafe is middle
RC_Trottle = 0; // RC input Channel 3 is for Trottle -> failsafe is middle
} // end of function MapRCtoFailsafe
int getChAmmount(void) { // function to return ammount of defined RC channels
return pwm_channel_count;
}
// supportfunctions to calibrate RC boundary values
#ifdef CHANNEL_CALIB
void CheckCentre(void) { // function to check the registered pwm pulse mid value in array
int range;
int midlow;
int midhigh;
for (int i = 0; i 3) {
Serial.print("Warning: midpoint value of cannel [");Serial.print(i);Serial.print("] is not in good range! -> Delta: ");Serial.println(range);
}
else {
Serial.print("Good mitpoint value of cannel [");Serial.print(i);Serial.print("] -> Delta: ");Serial.println(range);
}
}
} // end of function CheckCentre
void init_RCpwmRange(void) { // set min, max arrays to mit point value for range finding
for (int i = 0; i < pwm_channel_count; i++) {
PWminA[i] = 1500;
PWmaxA[i] = 1500;
}
} // end of function init_RCpwmRange
void print_RCpwmRange(int i){ // display the raw RC Channel PWM Inputs (min, actual, max)
if (PWminA[i] > pwmSigRaw[i]) {PWminA[i] = pwmSigRaw[i];} // buffer min and max values per Channel
if (PWmaxA[i] < pwmSigRaw[i]) {PWmaxA[i] = pwmSigRaw[i];}
int calcMP = PWminA[i] + ((PWmaxA[i] - PWminA[i]) / 2); // calc mid point out of min max values
// print out results
Serial.print(" CH");Serial.print(i+1);Serial.print(": ( ");
Serial.print(pwmSigRaw[i]);Serial.print(" ) { ");Serial.print(PWminA[i]);
Serial.print(", ");Serial.print(calcMP);Serial.print(", ");
Serial.print(PWmaxA[i]);Serial.print(" }");
i++;
if (i == (pwm_channel_count)) { Serial.println(); }
} // end of function print_RCpwmRange
#endif