pruio_pwmss.bas

Go to the documentation of this file.

' PruIo global declarations.
#INCLUDE ONCE "pruio_globals.bi"
' Header for PWMSS part, containing modules QEP, CAP and PWM.
#INCLUDE ONCE "pruio_pwmss.bi"
' driver header file
#INCLUDE ONCE "pruio.bi"


#DEFINE PWMSS_CLK  100e6

#DEFINE PWMSS_CLK_2 50e6



CONSTRUCTOR PwmssUdt(BYVAL T AS Pruio_ PTR)
  Top = T
  WITH *Top
    VAR i = .ParOffs
    InitParA = i
    i += 1 : .DRam[i] = &h48300000uL
    i += 1 : .DRam[i] = IIF(.DevAct AND PRUIO_ACT_PWM0, &h44E000D4uL, 0)

    i += 1 : .DRam[i] = &h48302000uL
    i += 1 : .DRam[i] = IIF(.DevAct AND PRUIO_ACT_PWM1, &h44E000CCuL, 0)

    i += 1 : .DRam[i] = &h48304000uL
    i += 1 : .DRam[i] = IIF(.DevAct AND PRUIO_ACT_PWM2, &h44E000D8uL, 0)

    .ParOffs = i
  END WITH
END CONSTRUCTOR



FUNCTION PwmssUdt.initialize CDECL() AS ZSTRING PTR
  WITH *Top
    VAR p_mem = .MOffs + .DRam[InitParA] _
      , p_raw = CAST(ANY PTR, .DRam) + PRUIO_DAT_PWM
    FOR i AS LONG = 0 TO PRUIO_AZ_PWMSS
      Raw(i) = p_raw
      Raw(i)->CMax = 0
      p_raw += SIZEOF(PwmssArr)

      Init(i) = p_mem
      Conf(i) = p_mem + .DSize

      WITH *Conf(i)
        IF .ClAd =  0 ORELSE _
           .IDVER = 0 THEN _ '                        subsystem disabled
                .DeAd = 0 : .ClVa = &h30000 : p_mem += 16 : _
                    Init(i)->DeAd = 0 : Init(i)->ClAd = 0 : CONTINUE FOR
        .ClVa = 2
        .SYSCONFIG = 2 SHL 2
        .CLKCONFIG = &b0100010001 ' enable all modules
        ' ePWM registers
        .TBPHS = 0
        .TBSTS = &b110
        .CMPCTL = 0
        ' eCAP registers
        .TSCTR = 0            ' reset counter
        .CTRPHS = 0
        .ECCTL1 = &b111001100
        .ECEINT = 0           ' disable all interupts
        .ECCLR = &b11111110   ' clear all interupt flags
      END WITH
      p_mem += SIZEOF(PwmssSet)
    NEXT
  END WITH : RETURN 0
END FUNCTION



FUNCTION PwmssUdt.cap_pwm_get CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL Freq AS Float_t PTR = 0 _
  , BYVAL Duty AS Float_t PTR = 0) AS ZSTRING PTR

  WITH *Conf(Nr)
    IF 2 <> .ClVa THEN                        Top->Errr = E0 : RETURN E0 ' PWM not enabled
    IF 0 = BIT(.ECCTL2, 9) THEN               Top->Errr = E9 : RETURN E9 ' eCAP module not in output mode
    IF Freq THEN *Freq = PWMSS_CLK / .CAP1
    IF Duty THEN *Duty = .CAP2 / .CAP1
  END WITH :                                                    RETURN 0
END FUNCTION



FUNCTION PwmssUdt.cap_pwm_set CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL F AS Float_t _
  , BYVAL D AS Float_t = 0.) AS ZSTRING PTR

  STATIC AS CONST Float_t _
    f_min = PWMSS_CLK / &hFFFFFFFFuL '' minimal frequency
  STATIC AS UInt32 _
    cnt(...) = {0, 0, 0} _  '' initial module periods
  , cmp(...) = {0, 0, 0}    '' initial module compares

  WITH *Top
    VAR r = 0
    IF 2 <> Conf(Nr)->ClVa THEN                   .Errr = E0 : RETURN E0 ' PWMSS not enabled
    IF 0 = cnt(Nr) ANDALSO _
       0 >= F THEN                                .Errr = E1 : RETURN E1 ' set frequency first
    IF F > 0. THEN
      IF F < f_min ORELSE _
         F > PWMSS_CLK_2 THEN                     .Errr = E2 : RETURN E2 ' frequency not supported
     cnt(Nr) = CUINT(PWMSS_CLK / F)
    END IF
    IF D >= 0 THEN cmp(Nr) = IIF(D > 1., cnt(Nr), CUINT(cnt(Nr) * D))

    Conf(Nr)->CAP1 = cnt(Nr)
    Conf(Nr)->CAP2 = cmp(Nr)
    IF Conf(Nr)->ECCTL2 <> PwmMode THEN
      Conf(Nr)->ECCTL2 = PwmMode
      Raw(Nr)->CMax = 0
      r = PwmMode
    END IF

    IF .DRam[0] > PRUIO_MSG_IO_OK THEN                          RETURN 0

    PruReady(1) ' wait, if PRU is busy (should never happen)
    .DRam[5] = 0 ' counter start value
    .DRam[4] = cmp(Nr)
    .DRam[3] = cnt(Nr)
    .DRam[2] = Conf(Nr)->DeAd + &h100
    .DRam[1] = r OR (PRUIO_COM_CAP_PWM SHL 24)
  END WITH :                                                    RETURN 0
END FUNCTION



FUNCTION PwmssUdt.cap_tim_get CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL Dur1 AS Float_t PTR _
  , BYVAL Dur2 AS Float_t PTR) AS ZSTRING PTR

  IF Dur1 = 0 ORELSE Dur2 = 0 THEN Top->Errr = @"pass pointers" : RETURN Top->Errr
  WITH *Conf(Nr)
    VAR dur = *Dur1 + *Dur2 _ ' [mSec]
      , dmx = 1000. * &hFFFFFFFF / PWMSS_CLK _
      , dmn = 1000. * 2 / PWMSS_CLK
    SELECT CASE dur
    CASE IS <= 0. ' get current
      IF 2 <> .ClVa THEN                         Top->Errr = E0 : RETURN E0 ' PWMSS not enabled
      IF 0 = BIT(.ECCTL2, 9) THEN                Top->Errr = E9 : RETURN E9 ' eCAP module not in output mode
      IF BIT(.ECCTL2, 4) THEN ' running
        *Dur2 = 1000. * .CAP2 / PWMSS_CLK
        *Dur1 = 1000. * .CAP1 / PWMSS_CLK - *Dur2
      ELSE ' stopped
        *Dur2 = 0.
        *Dur1 = 0.
      END IF
    CASE IS < dmn ' get minimal
      *Dur1 = dmn
      *Dur2 = dmn / 2
    CASE IS > dmx ' get maximal
      *Dur1 = *Dur1 / dur * dmx
      *Dur2 = *Dur2 / dur * dmx
    CASE ELSE
      *Dur1 = 1000. * CULNG(*Dur1 / dur * PWMSS_CLK)
      *Dur2 = 1000. * CULNG(*Dur2 / dur * PWMSS_CLK)
    END SELECT
  END WITH :                                                     RETURN 0
END FUNCTION



FUNCTION PwmssUdt.cap_tim_set CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL Dur1 AS Float_t _
  , BYVAL Dur2 AS Float_t _
  , BYVAL Mode AS SHORT) AS ZSTRING PTR

  STATIC AS CONST Float_t _
    d_min = 1000. *           &h2 / PWMSS_CLK _ '' minimal durarion [mSec]
  , d_max = 1000. * &hFFFFFFFFuLL / PWMSS_CLK   '' maximal duration [mSec]

  VAR dur = Dur1 + Dur2 ' [msec]
  WITH *Conf(Nr)
    IF 2 <> .ClVa                           THEN Top->Errr = E0 : RETURN E0 ' PWMSS not enabled

    Raw(Nr)->CMax = 0
    SELECT CASE dur
    CASE IS <= 0. ' -> switch off
      .CAP1 = &hFFFFFFFFuL
      .CAP2 = &h0
      .ECCTL2 = IIF(Mode AND &b01, PwmMode OR &b10000000000, PwmMode) ' default/invers
    CASE IS < d_min :                Top->Errr = Top->TimSS->E3 : RETURN Top->Errr ' duration too short
    CASE IS > d_max :                Top->Errr = Top->TimSS->E4 : RETURN Top->Errr ' duration too long
    CASE ELSE
      IF Mode >= &b10 THEN ' one-shot
        IF 0.01 >= Dur1 THEN         Top->Errr = Top->TimSS->E3 : RETURN Top->Errr ' one-shot duration too short
        Raw(Nr)->CMax = (Mode AND &b111111110) SHR 1
        .ECCTL2 = PwmMode OR IIF(Mode AND &b01, &b10010000000, &b00010000000) ' default/invers
      ELSE
        .ECCTL2 = IIF(Mode AND &b01, PwmMode OR &b10000000000, PwmMode) ' default/invers
      END IF
      .CAP1 = CULNG(.001 * dur * PWMSS_CLK) ' period
      VAR x = CULNG(Dur2 / dur * .CAP1) '     match
      .CAP2 = IIF(x > 0, x, 1)
    END SELECT
    .TSCTR = .CAP2
  END WITH

  WITH *Top
    IF .DRam[0] > PRUIO_MSG_IO_OK                            THEN RETURN 0

    PruReady(1) ' wait, if PRU is busy (should never happen)
    .DRam[5] = Conf(Nr)->TSCTR
    .DRam[4] = Conf(Nr)->CAP2
    .DRam[3] = Conf(Nr)->CAP1
    .DRam[2] = Conf(Nr)->DeAd + &h100
    .DRam[1] = Conf(Nr)->ECCTL2 OR (PRUIO_COM_CAP_TIM SHL 24)
  END WITH :                                                      RETURN 0
END FUNCTION



FUNCTION PwmssUdt.pwm_pwm_get CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL F AS Float_t PTR = 0 _
  , BYVAL Du AS Float_t PTR = 0  _
  , BYVAL Mo AS UInt8) AS ZSTRING PTR

  WITH *Conf(Nr)
    IF 2 <> .ClVa THEN                        Top->Errr = E0 : RETURN E0 ' PWMSS not enabled
    VAR p = CAST(UInt32, .TBPRD)
    IF F THEN
      VAR d1 = (.TBCTL SHR 7) AND &b111, d2 = (.TBCTL SHR 10) AND &b111
      VAR cg = p * IIF(d1, d1 SHL 1, 1) * (1 SHL d2)
      *F = PWMSS_CLK / IIF(BIT(.TBCTL, 1), cg SHL 1, cg)
    END IF
    IF Du THEN
      VAR c = CAST(UInt32, IIF(Mo, .CMPB, .CMPA))
      IF BIT(.TBCTL, 1) THEN '                             count up-down
        p SHL= 1
        IF IIF(Mo, .AQCTLB, .AQCTLA) AND &b010001000000 THEN c = p - c
      END IF
      *Du = c / p
    END IF
  END WITH :                                                    RETURN 0
END FUNCTION




FUNCTION PwmssUdt.pwm_pwm_set CDECL( _
    BYVAL Nr AS UInt8 _
  , BYVAL F AS Float_t _
  , BYVAL Da AS Float_t = 0. _
  , BYVAL Db AS Float_t = 0.) AS ZSTRING PTR

  STATIC AS CONST Float_t _
    f_min = PWMSS_CLK_2 / &h6FFF900   '' minimal frequency (128 * 14 * 65535)
  STATIC AS Float_t _
   freq(...) = {0., 0., 0.} _ ' module frequencies
  , d_a(...) = {0., 0., 0.} _ ' module duty A
  , d_b(...) = {0., 0., 0.}   ' module duty B
  STATIC AS UInt16 _
    cnt(...) = {0, 0, 0} _ ' module periods
  , c_a(...) = {0, 0, 0} _ ' module counters A
  , c_b(...) = {0, 0, 0}   ' module counters B

  VAR ctl = 0
  WITH *Conf(Nr)
    IF 2 <> .ClVa THEN                        Top->Errr = E0 : RETURN E0 ' PWMSS not enabled
    IF 0 = cnt(Nr) THEN
      IF F <= 0. THEN                         Top->Errr = E1 : RETURN E1 ' set frequency first
    ELSE
      IF F > 0. ANDALSO freq(Nr) <> F THEN cnt(Nr) = 0
    END IF

    IF 0 = cnt(Nr) THEN '                    calc new period (frequency)
      VAR cycle = IIF(F > f_min ANDALSO F <= PWMSS_CLK_2, CUINT(.5 + PWMSS_CLK / F), 0uL)
      IF 2 > cycle THEN                       Top->Errr = E2 : RETURN E2 ' frequency not supported

      freq(Nr) = ABS(F)
      IF cycle <= &h10000 ANDALSO 0 = BIT(Top->Pwm->ForceUpDown, Nr) THEN ' count up mode
        cnt(Nr) = cycle
      ELSEIF cycle < &h20000 THEN '        no divisor count up-down mode
        cnt(Nr) = cycle SHR 1
        ctl = 2
      ELSE '                                  divisor count up-down mode
        VAR fac = cycle SHR 17 _
           , x1 = 1 + CINT(INT(LOG(fac / 14) / LOG2))
        IF x1 < 0 THEN x1 = 0
        VAR x2 = (fac SHR x1) \ 2 + 1
        cnt(Nr) = CUINT(.5 + PWMSS_CLK_2 / F / (IIF(x2, x2 SHL 1, 1) SHL x1))
        ctl = 2 + x2 SHL 7 + x1 SHL 10 ' clock divisor
      END IF
      ctl OR= (3 SHL 14) + (Top->Pwm->Cntrl(Nr) AND &b0010000001111100)
      .TBCTL = ctl
      .TBPRD = cnt(Nr)
      .TBCNT = 0
      c_a(Nr) = 0
      c_b(Nr) = 0
    END IF

    IF Da >= 0. THEN d_a(Nr) = IIF(Da > 1., 1., Da) : c_a(Nr) = 0
    IF 0 = c_a(Nr) THEN '                     calc new duty for A output
      IF BIT(.TBCTL, 1) ORELSE BIT(Top->Pwm->ForceUpDown, Nr) THEN 'up-down mode
        IF d_a(Nr) >= .5 _
          THEN .AQCTLA = Top->Pwm->AqCtl(0, Nr, 2) : c_a(Nr) = CUINT((cnt(Nr) SHL 1) * (1 - d_a(Nr))) _
          ELSE .AQCTLA = Top->Pwm->AqCtl(0, Nr, 1) : c_a(Nr) = CUINT((cnt(Nr) SHL 1) * d_a(Nr))
      ELSE '                                                        up mode
               .AQCTLA = Top->Pwm->AqCtl(0, Nr, 0) : c_a(Nr) = CUINT(.5 + (cnt(Nr) + 1) * d_a(Nr))
      END IF
      .CMPA = c_a(Nr)
    END IF

    IF Db >= 0. THEN d_b(Nr) = IIF(Db > 1., 1., Db) : c_b(Nr) = 0
    IF 0 = c_b(Nr) THEN '                     calc new duty for B output
      IF BIT(.TBCTL, 1) ORELSE BIT(Top->Pwm->ForceUpDown, Nr) THEN ' up-down mode
        IF d_b(Nr) >= .5 _
          THEN .AQCTLB = Top->Pwm->AqCtl(1, Nr, 2) : c_b(Nr) = CUINT((cnt(Nr) SHL 1) * (1 - d_b(Nr))) _
          ELSE .AQCTLB = Top->Pwm->AqCtl(1, Nr, 1) : c_b(Nr) = CUINT((cnt(Nr) SHL 1) * d_b(Nr))
      ELSE '                                                         up mode
               .AQCTLB = Top->Pwm->AqCtl(1, Nr, 0) : c_b(Nr) = CUINT(.5 + (cnt(Nr) + 1) * d_b(Nr))
      END IF
      .CMPB = c_b(Nr)
    END IF

    IF Top->DRam[0] > PRUIO_MSG_IO_OK THEN                      RETURN 0

    WHILE Top->DRam[1] : WEND ' wait, if PRU is busy (should never happen)
    Top->DRam[5] = .TBCNT + .TBPRD SHL 16
    Top->DRam[4] = .AQCTLA + .AQCTLB SHL 16
    Top->DRam[3] = .CMPA + .CMPB SHL 16
    Top->DRam[2] = .DeAd + &h200
    Top->DRam[1] = IIF(ctl, ctl, 0) + PRUIO_COM_PWM SHL 24
  END WITH :                                                    RETURN 0
END FUNCTION



CONSTRUCTOR PwmMod(BYVAL T AS Pruio_ PTR)
  Top = T
END CONSTRUCTOR



FUNCTION PwmMod.Value CDECL( _
    BYVAL Ball AS UInt8 _
  , BYVAL Hz AS Float_t PTR = 0 _
  , BYVAL Du AS Float_t PTR = 0) AS ZSTRING PTR

  WITH *Top
    DIM AS ZSTRING PTR e
    SELECT CASE AS CONST Ball
    CASE &h24 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .TimSS->pwm_get(0, Hz, Du))        ' P8_07
    CASE &h27 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .TimSS->pwm_get(1, Hz, Du))        ' P8_09
    CASE &h26 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .TimSS->pwm_get(2, Hz, Du))        ' P8_10
    CASE &h25 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .TimSS->pwm_get(3, Hz, Du))        ' P8_08
    CASE &h09 : e = IIF(ModeCheck(Ball,4), .PwmSS->E3, .PwmSS->pwm_pwm_get(2, Hz, Du, 1)) ' P8_13
    CASE &h08 : e = IIF(ModeCheck(Ball,4), .PwmSS->E3, .PwmSS->pwm_pwm_get(2, Hz, Du, 0)) ' P8_19
    CASE &h33 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .PwmSS->pwm_pwm_get(1, Hz, Du, 1)) ' P8_34
    CASE &h32 : e = IIF(ModeCheck(Ball,2), .PwmSS->E3, .PwmSS->pwm_pwm_get(1, Hz, Du, 0)) ' P8_36
    CASE &h28 : e = IIF(ModeCheck(Ball,3), .PwmSS->E3, .PwmSS->pwm_pwm_get(2, Hz, Du, 1)) ' P8_45
    CASE &h29 : e = IIF(ModeCheck(Ball,3), .PwmSS->E3, .PwmSS->pwm_pwm_get(2, Hz, Du, 0)) ' P8_46
    CASE &h12 : e = IIF(ModeCheck(Ball,6), .PwmSS->E3, .PwmSS->pwm_pwm_get(1, Hz, Du, 0)) ' P9_14
    CASE &h13 : e = IIF(ModeCheck(Ball,6), .PwmSS->E3, .PwmSS->pwm_pwm_get(1, Hz, Du, 1)) ' P9_16
    CASE &h55 : e = IIF(ModeCheck(Ball,3), .PwmSS->E3, .PwmSS->pwm_pwm_get(0, Hz, Du, 1)) ' P9_21
    CASE &h54 : e = IIF(ModeCheck(Ball,3), .PwmSS->E3, .PwmSS->pwm_pwm_get(0, Hz, Du, 0)) ' P9_22
    CASE &h65 : e = IIF(ModeCheck(Ball,1), .PwmSS->E3, .PwmSS->pwm_pwm_get(0, Hz, Du, 1)) ' P9_29
    CASE &h64 : e = IIF(ModeCheck(Ball,1), .PwmSS->E3, .PwmSS->pwm_pwm_get(0, Hz, Du, 0)) ' P9_31
    CASE &h67 : e = IIF(ModeCheck(Ball,4), .PwmSS->E3, .PwmSS->cap_pwm_get(2, Hz, Du))    ' P9_28
    CASE &h5D : e = IIF(ModeCheck(Ball,4), .PwmSS->E3, .PwmSS->cap_pwm_get(1, Hz, Du))    ' JT_05
    CASE &h59 : e = IIF(ModeCheck(Ball,0), .PwmSS->E3, .PwmSS->cap_pwm_get(0, Hz, Du))    ' P9_42
    'CASE P8_15 : e = IIF(ModeCheck(Ball,5), .PwmSS->E3, pru_cap_get(Hz, Du))
    CASE ELSE  : e = .PwmSS->E4
    END SELECT : IF e THEN .Errr = e :                      RETURN .Errr
  END WITH : RETURN 0
END FUNCTION



FUNCTION PwmMod.setValue CDECL( _
  BYVAL Ball AS UInt8, _
  BYVAL Hz AS Float_t, _
  BYVAL Du AS Float_t) AS ZSTRING PTR

  WITH *Top
    SELECT CASE AS CONST Ball
    CASE &h24 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_07
      RETURN .TimSS->pwm_set(0, Hz, Du)
    CASE &h27 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_09
      RETURN .TimSS->pwm_set(1, Hz, Du)
    CASE &h26 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_10
      RETURN .TimSS->pwm_set(2, Hz, Du)
    CASE &h25 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_08
      RETURN .TimSS->pwm_set(3, Hz, Du)
    CASE &h09 : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h0C) ' P8_13
      RETURN .PwmSS->pwm_pwm_set(2, Hz, -1., Du)
    CASE &h08 : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h0C) ' P8_19
      RETURN .PwmSS->pwm_pwm_set(2, Hz, Du, -1.)
    CASE &h33 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_34
      RETURN .PwmSS->pwm_pwm_set(1, Hz, -1., Du)
    CASE &h32 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' P8_36
      RETURN .PwmSS->pwm_pwm_set(1, Hz, Du, -1.)
    CASE &h28 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h0B) ' P8_45
      RETURN .PwmSS->pwm_pwm_set(2, Hz, -1., Du)
    CASE &h29 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h0B) ' P8_46
      RETURN .PwmSS->pwm_pwm_set(2, Hz, Du, -1.)
    CASE &h12 : IF ModeCheck(Ball,6) THEN ModeSet(Ball, &h0E) ' P9_14
      RETURN .PwmSS->pwm_pwm_set(1, Hz, Du, -1.)
    CASE &h13 : IF ModeCheck(Ball,6) THEN ModeSet(Ball, &h0E) ' P9_16
      RETURN .PwmSS->pwm_pwm_set(1, Hz, -1., Du)
    CASE &h55 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h0B) ' P9_21
      RETURN .PwmSS->pwm_pwm_set(0, Hz, -1., Du)
    CASE &h54 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h0B) ' P9_22
      RETURN .PwmSS->pwm_pwm_set(0, Hz, Du, -1.)
    CASE &h65 : IF ModeCheck(Ball,1) THEN ModeSet(Ball, &h09) ' P9_29
      RETURN .PwmSS->pwm_pwm_set(0, Hz, -1., Du)
    CASE &h64 : IF ModeCheck(Ball,1) THEN ModeSet(Ball, &h09) ' P9_31
      RETURN .PwmSS->pwm_pwm_set(0, Hz, Du, -1.)
    CASE &h67 : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h0C) ' P9_28
      RETURN .PwmSS->cap_pwm_set(2, Hz, Du)
    CASE &h59 : IF ModeCheck(Ball,0) THEN ModeSet(Ball, &h08) ' P9_42
      RETURN .PwmSS->cap_pwm_set(0, Hz, Du)
    CASE &h5D : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h0C) ' JT_05
      RETURN .PwmSS->cap_pwm_set(1, Hz, Du)
    CASE &h58 : IF ModeCheck(Ball,2) THEN ModeSet(Ball, &h0A) ' SD_10
      RETURN .PwmSS->cap_pwm_set(1, Hz, Du)
    'CASE P8_15 : IF ModeCheck(Ball,5) THEN ModeSet(Ball, &h0D)
      'RETURN pru_cap_set(Hz, Du)
    END SELECT :                       .Errr = .PwmSS->E4 : RETURN .Errr ' pin has no PWM capability
  END WITH
END FUNCTION




FUNCTION PwmMod.Sync CDECL( _
  BYVAL Mask AS UInt8) AS ZSTRING PTR
  WITH *Top
    IF .MuxFnr < 1 ORELSE .MuxFnr > 255                      THEN RETURN @"libpruio LKM missing"
    PRINT #.MuxFnr, "FF" & HEX(Mask, 2) : SEEK #.MuxFnr, 1      : RETURN 0
  END WITH
END FUNCTION

'FUNCTION PwmMod.SyncToggle CDECL( _
  'BYVAL Mask AS UInt8 = 0,
  'BYVAL Curr AS UInt8 PTR = 0) AS ZSTRING PTR
  'WITH *Top
    'if Curr then
      'PruReady(1) ' wait if PRU busy
      '.DRam[2] = &h44E10664uL
      '.DRam[1] = 4 OR PRUIO_COM_PEEK
      'PruReady(1) ' wait if PRU busy
      
    'end if
    'if Mask then
      'IF .MuxFnr < 1 ORELSE .MuxFnr > 255                    THEN RETURN @"libpruio LKM missing"

      'PRINT #.MuxFnr, "FF" & HEX(Mask, 2) : SEEK #.MuxFnr, 1    : RETURN 0
    'END if
  'END WITH
'END FUNCTION



CONSTRUCTOR CapMod(BYVAL T AS Pruio_ PTR)
  Top = T
END CONSTRUCTOR



FUNCTION CapMod.config CDECL( _
    BYVAL Ball AS UInt8 _
  , BYVAL FLow AS Float_t = 0.) AS ZSTRING PTR

  VAR m = 0
  WITH *Top
    SELECT CASE AS CONST Ball
    CASE &h67 : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h24) ' P9_28, P2_30, GP0_6
      m = 2
    CASE &h5C : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h24) ' JT_04, P1_32, UT0_3
      m = 2
    CASE &h59 : IF ModeCheck(Ball,0) THEN ModeSet(Ball, &h20) ' P9_42, P2_29, SPI2_6
    CASE &h58 : IF ModeCheck(Ball,1) THEN ModeSet(Ball, &h22) ' SD_10
      m = 1
    CASE &h5D : IF ModeCheck(Ball,4) THEN ModeSet(Ball, &h24) ' JT_05, P1_30, UT0_4
      m = 1
    'CASE P8_15 : IF ModeCheck(Ball,5) THEN ModeSet(Ball, &h25) ' pr1_ecap0_ecap_capin_apwm_o (also on P9_42)
    'CASE    98 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h23)
      'm = 2
    'CASE    99 : IF ModeCheck(Ball,3) THEN ModeSet(Ball, &h23)
      'm = 1
    CASE ELSE                              : .Errr = .PwmSS->E6 : RETURN .Errr ' pin has no CAP capability
    END SELECT
    IF 2 <> .PwmSS->Conf(m)->ClVa       THEN .Errr = .PwmSS->E0 : RETURN .Errr ' PWMSS not enabled
    WITH *.PwmSS
      VAR cnt = &hFFFFFFFFul
      IF FLow > PWMSS_CLK/ &hFFFFFFFFul THEN
        cnt = CUINT(PWMSS_CLK / FLow)
        IF cnt < 256 THEN cnt = 256
      END IF
      .Raw(m)->CMax = cnt
      .Conf(m)->ECCTL2 = .CapMode
    END WITH

    IF .DRam[0] > PRUIO_MSG_IO_OK                            THEN RETURN 0

    PruReady(1) ' wait, if PRU is busy
    .DRam[2] = .PwmSS->Conf(m)->DeAd + &h100
    .DRam[1] = .PwmSS->CapMode + (PRUIO_COM_CAP SHL 24)
  END WITH :                                                      RETURN 0
END FUNCTION



FUNCTION CapMod.Value CDECL( _
    BYVAL Ball AS UInt8 _
  , BYVAL Hz AS Float_t PTR = 0 _
  , BYVAL Du AS Float_t PTR = 0) AS ZSTRING PTR

  VAR m = 0
  WITH *Top
    DIM AS ZSTRING PTR e
    IF 2 <> .PwmSS->Conf(m)->ClVa THEN
      e = .PwmSS->E0 ' PWMSS not enabled
    ELSE
      SELECT CASE AS CONST Ball
      CASE &h67 : IF ModeCheck(Ball,4) THEN e = .PwmSS->E5 ELSE m = 2 ' P9_28, P2_30, GP0_6
      CASE &h59 : IF ModeCheck(Ball,0) THEN e = .PwmSS->E5 ' P9_42, P2_29, SPI2_6
      CASE &h5C : IF ModeCheck(Ball,4) THEN e = .PwmSS->E5 ELSE m = 2 ' JT_04, P1_32, UT0_3
      CASE &h58 : IF ModeCheck(Ball,1) THEN e = .PwmSS->E5 ELSE m = 1 ' SD_10
      CASE &h5D : IF ModeCheck(Ball,4) THEN e = .PwmSS->E5 ELSE m = 1 ' JT_04, P1_32, UT0_3
      'CASE P8_15 : IF ModeCheck(Ball,5) THEN e = .PwmSS->E5 ELSE m = -1 ' pr1_ecap0_ecap_capin_apwm_o (also on P9_42)
      'CASE 98 : IF ModeCheck(Ball,3) THEN e = .PwmSS->E5 ELSE m = 2
      'CASE 99 : IF ModeCheck(Ball,3) THEN e = .PwmSS->E5 ELSE m = 1
      CASE ELSE  : e = .PwmSS->E6 ' pin has no CAP capability
      END SELECT
    END IF : IF e THEN .Errr = e                          : RETURN .Errr

    IF .DRam[0] > PRUIO_MSG_IO_OK THEN
      IF Hz THEN *Hz = 0
      IF Du THEN *Du = 0
                        .Errr = @"IO/RB mode not running" : RETURN .Errr
    END IF
  END WITH
  WITH *Top->PwmSS->Raw(m)
    IF .CMax THEN
      IF Hz THEN *Hz = IIF(.C2, PWMSS_CLK / .C2, 0.)
      IF Du THEN *Du = IIF(.C2, (.C2 - .C1) / .C2, 0.)
                                                                RETURN 0
    END IF
    IF Hz THEN *Hz = 0
    IF Du THEN *Du = 0
    Top->Errr = Top->PwmSS->E5                        : RETURN Top->Errr ' pin not in CAP mode
  END WITH
END FUNCTION




CONSTRUCTOR QepMod(BYVAL T AS Pruio_ PTR)
  Top = T
END CONSTRUCTOR



FUNCTION QepMod.config CDECL( _
    BYVAL Ball AS UInt8 _
  , BYVAL PMax AS UInt32 = &h7FFFFFFFul _
  , BYVAL VHz AS Float_t = 25. _
  , BYVAL Scale AS Float_t = 1. _
  , BYVAL Mo AS UInt8 = 0) AS ZSTRING PTR

  VAR m = 0, x = 0
  STATIC AS Float_t fmin = PWMSS_CLK / (&hFFFF SHL 7) ' minimal frequency
  WITH *Top
    IF VHz < fmin ORELSE VHz > PWMSS_CLK_2 THEN _
                                          .Errr = .PwmSS->E2 : RETURN .Errr ' frequency not supported
    SELECT CASE AS CONST Ball
    CASE &h0D, &h0C, &h0E : m = 2 ' P8_11, P8_12, P8_16
      VAR v = IIF(Mo = PRUIO_PIN_RESET, PRUIO_PIN_RESET, &h2C)
      IF ModeCheck(&h0C,4) THEN ModeSet(&h0C,v)
      IF Ball = &h0C THEN                       x = 2 : EXIT SELECT ' P8_12
      IF ModeCheck(&h0D,4) THEN ModeSet(&h0D,v)
      IF Ball = &h0D THEN                       x = 1 : EXIT SELECT ' P8_11
      IF ModeCheck(&h0E,4) THEN ModeSet(&h0E,v)
    CASE &h14 : m = 1 ' P2_10
      VAR v = IIF(Mo = PRUIO_PIN_RESET, PRUIO_PIN_RESET, &h2E)
      IF ModeCheck(&h14,6) THEN ModeSet(&h14,v)
      x = 2
    CASE &h35, &h34, &h36 : m = 1 ' P8_33, P8_35, P8_31
      VAR v = IIF(Mo = PRUIO_PIN_RESET, PRUIO_PIN_RESET, &h2A)
      IF ModeCheck(&h34,2) THEN ModeSet(&h34,v)
      IF Ball = &h34 THEN                       x = 2 : EXIT SELECT ' P8_35
      IF ModeCheck(&h35,2) THEN ModeSet(&h35,v)
      IF Ball = &h35 THEN                       x = 1 : EXIT SELECT ' P8_33
      IF ModeCheck(&h36,2) THEN ModeSet(&h36,v)
    CASE &h2C, &h2D, &h2E : m = 2 ' P8_41, P8_42, P8_39
      VAR v = IIF(Mo = PRUIO_PIN_RESET, PRUIO_PIN_RESET, &h2B)
      IF ModeCheck(&h2C,3) THEN ModeSet(&h2C,v)
      IF Ball = &h2C THEN                       x = 2 : EXIT SELECT ' P8_41
      IF ModeCheck(&h2E,3) THEN ModeSet(&h2E,v)
      IF Ball = &h2D THEN                       x = 1 : EXIT SELECT ' P8_42
      IF ModeCheck(&h2E,3) THEN ModeSet(&h2E,v)
    CASE &h69, &h59, &h68, &h6D, &h6A : m = 0 ' P9_27, P9_42, 104, P9_41, 106
      VAR v = IIF(Mo = PRUIO_PIN_RESET, PRUIO_PIN_RESET, &h29)
      IF ModeCheck(&h68 ,1) THEN ModeSet(&h68 ,v)
      IF Ball = &h59 ORELSE Ball = &h68 THEN     x = 2 : EXIT SELECT
      IF ModeCheck(&h69,1) THEN ModeSet(&h69,v)
      IF Ball = &h69 THEN                        x = 1 : EXIT SELECT
      IF ModeCheck(&h6A ,1) THEN ModeSet(&h6A ,v)
    CASE ELSE :                           .Errr = .PwmSS->E8 : RETURN .Errr' pin has no QEP capability
    END SELECT
    IF 2 <> .PwmSS->Conf(m)->ClVa THEN    .Errr = .PwmSS->E0 : RETURN .Errr' PWMSS not enabled
  END WITH
  WITH *Top->PwmSS->Conf(m)
    .QPOSCNT = 0
    .QPOSINIT = 0
    .QPOSMAX = PMax
    .QPOSLAT = 0
    .QUTMR = 0
    .QUPRD = cuint(PWMSS_CLK / VHz)

    VAR ccps = .QUPRD \ &h10000
    IF ccps > 1 THEN ccps = 1 + INT(LOG(ccps) / LOG2)
    SELECT CASE AS CONST x
    CASE 2 '                                               up count mode
      .QDECCTL = &b1000000000000000
      .QEPCTL  = &b0001000010001110
      .QCAPCTL = &b1000000000000000 or (ccps shl 4)
    CASE 1 '                                        direction count mode
      .QDECCTL = &b0000000000000000
      .QEPCTL  = &b0001000010001110
      .QCAPCTL = &b1000000000000010 or (ccps shl 4)
    CASE ELSE '                          direction count mode with index
      .QDECCTL = &b0000000000000000
      .QEPCTL  = &b0000001010001110
      .QCAPCTL = &b1000000000000010 or (ccps shl 4)
    END SELECT
    .QPOSCTL = &b0000000000000000
    .QCTMR = 0
    .QCPRD = 0

    VAR fx = 1 SHL ((.QCAPCTL SHR 4) AND &b111) _
      , fp = 1 SHL (.QCAPCTL AND &b1111) _
      , t = .QUPRD \ fx _
      , p2 = fp / 2
    FVh(m) = Scale * PWMSS_CLK / .QUPRD
    FVl(m) = Scale * PWMSS_CLK / fx * fp
    Prd(m) = CUINT(fp * t / (-p2 + SQR(p2 * p2 + t / fp))) SHL 16
    IF Top->DRam[0] > PRUIO_MSG_IO_OK THEN                     RETURN 0

    WHILE Top->DRam[1] : WEND '                     wait, if PRU is busy
    Top->DRam[6] = .QCAPCTL
    Top->DRam[5] = .QDECCTL OR .QEPCTL SHL 16
    Top->DRam[4] = .QUPRD
    Top->DRam[3] = .QPOSMAX
    Top->DRam[2] = .DeAd + &h180
    Top->DRam[1] = PRUIO_COM_QEP SHL 24
  END WITH :                                                   RETURN 0
END FUNCTION



FUNCTION QepMod.Value CDECL( _
    BYVAL Ball AS UInt8 _
  , BYVAL Posi AS UInt32 PTR = 0 _
  , BYVAL Velo AS Float_t PTR = 0) AS ZSTRING PTR

  VAR m = 0
  WITH *Top
    SELECT CASE AS CONST Ball
    CASE &h14       : Posi = 0 : m = 1 '   A: P2_10
    CASE &h0C       : Posi = 0 : m = 2 '   A: P8_12
    CASE &h0D, &h0E            : m = 2 ' B,I: P8_11, P8_16
    CASE &h34       : Posi = 0 : m = 1 '   A: P8_35
    CASE &h35, &h36            : m = 1 ' B,I: P8_33, P8_31
    CASE &h2C       : Posi = 0 : m = 2 '   A: P8_41
    CASE &h2D, &h2E            : m = 2 ' B,I: P8_42, P8_39
    CASE &h59, &h68 : Posi = 0 : m = 0 '   A: P9_42, 104
    CASE &h69, &h6D, &h6A      : m = 0 ' B,I: P9_27, P9_41, 106
    CASE ELSE  :                       .Errr = .PwmSS->E0 : RETURN .Errr 'PwmSS not enabled
    END SELECT

    IF .DRam[0] > PRUIO_MSG_IO_OK THEN
      IF Posi THEN *Posi = 0
      IF Velo THEN *Velo = 0.
                        .Errr = @"IO/RB mode not running" : RETURN .Errr
    END IF
  END WITH
  WITH *Top->PwmSS->Raw(m)
    IF Velo THEN
      VAR dx = CINT(.NPos - .OPos)
      IF .PLat > Prd(m) THEN
        *Velo = dx * FVh(m)
      ELSE
        *Velo = IIF(HIWORD(.PLat), SGN(dx) * FVl(m) / HIWORD(.PLat), 0.)
      END IF
    END IF
    IF Posi THEN *Posi = .QPos
  END WITH :                                                    RETURN 0
END FUNCTION