src/test/test_ota/crsf_sysmocks.cpp

   1 #include "crsf_sysmocks.h"
   2 #include "crsf_protocol.h"
   3 /**
   4  * Functions to replicate the CRSF conversion routines in OpenTX / Betaflight
   5  **/
   6 extern "C" {
   7
   8 #define limit(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
   9 #define CROSSFIRE_CENTER            0x3E0 // 992
  10 #define CROSSFIRE_CENTER_CH_OFFSET  (0)
  11 #define PPM_CENTER                  1500
  12 #define PPM_CH_CENTER               (PPM_CENTER)
  13
  14 uint32_t OpenTx_to_Crsf(int16_t pulse)
  15 {
  16     uint32_t val = limit(0, CROSSFIRE_CENTER + (CROSSFIRE_CENTER_CH_OFFSET * 4) / 5 + (pulse * 4) / 5, 2 * CROSSFIRE_CENTER);
  17     return val;
  18 }
  19
  20 int16_t Crsf_to_OpenTx(uint32_t crsfval)
  21 {
  22     int16_t val = (crsfval - CROSSFIRE_CENTER - (CROSSFIRE_CENTER_CH_OFFSET * 5 / 4)) * 5 / 4;
  23     return val;
  24 }
  25
  26 uint32_t OpenTx_to_Us(int16_t pulse)
  27 {
  28     return PPM_CENTER + (pulse / 2);
  29 }
  30
  31 int16_t Us_to_OpenTx(uint32_t us)
  32 {
  33     return 2 * ((int32_t)(us) - PPM_CENTER);
  34 }
  35
  36 uint32_t Crsf_to_OpenTx_to_Us(uint32_t crsfval)
  37 {
  38     return OpenTx_to_Us(Crsf_to_OpenTx(crsfval));
  39 }
  40
  41 uint32_t Crsf_to_Uint10_to_Crsf(uint32_t crsfval)
  42 {
  43     return UINT10_to_CRSF(CRSF_to_UINT10(crsfval));
  44 }
  45
  46 #define CRSF_RC_CHANNEL_SCALE_LEGACY                0.62477120195241f
  47 uint32_t Crsf_to_BfUs(uint32_t crsfval)
  48 {
  49     float val = (CRSF_RC_CHANNEL_SCALE_LEGACY * (float)crsfval) + 881;
  50     //printf("%u=%f ", crsfval, val);
  51     return val;
  52 }
  53
  54 } // extern "C"