<commit_before>{-# LANGUAGE DataKinds #-}
{-# LANGUAGE TypeOperators #-}
{-# LANGUAGE FlexibleInstances #-}
{-# LANGUAGE QuasiQuotes #-}

module UserInputDecode where

import Ivory.Language

import IvoryHelpers

import qualified UserInputType as I

userInputDecodeModule :: Module
userInputDecodeModule = package "userinput_decode" $ do
  depend I.userInputModule
  defStruct (Proxy :: Proxy "userinput_decode_state")
  incl decode
  incl scale_proc

[ivory|
struct userinput_decode_state
  { last_modepwm      :: Stored Uint16
  ; last_modepwm_time :: Stored Uint32
  ; arm_state         :: Stored Uint8
  ; arm_state_time    :: Stored Uint32
  }
|]

as_DISARMED, as_ARMING, as_ARMED :: Uint8
as_DISARMED = 0
as_ARMING   = 1
as_ARMED    = 2

mode_STABILIZE, mode_ALT_HOLD, mode_LOITER :: Uint8
mode_STABILIZE = 0
mode_ALT_HOLD  = 1
mode_LOITER    = 2

mode_pwm_map :: [(Uint8, (Uint16, Uint16))]
mode_pwm_map = [(mode_STABILIZE, (900, 1300))
               ,(mode_ALT_HOLD,  (1301, 1700))
               ,(mode_LOITER,    (1701, 2100))
               ]

decode :: Def ('[ Ref s1 (Array 8 (Stored Uint16))
                , Ref s2 (Struct "userinput_decode_state")
                , Ref s3 (Struct "userinput_result")
                , Uint32 ] :-> ())
decode = proc "userinput_decode" $ \pwms state out now -> body $ do
  let chtransform ix f ofield = deref (pwms ! (ix :: Ix Uint8 8)) >>=
                                f >>= \v -> store (out ~> ofield) v
  store (out ~> I.time) now
  chtransform 0 scale_rpy I.roll
  chtransform 1 scale_rpy I.pitch
  chtransform 2 scale_thr I.throttle
  chtransform 3 scale_rpy I.yaw
  arming_statemachine pwms state out now
  mode_statemachine pwms state out now
  retVoid

arming_statemachine :: (Ref s1 (Array 8 (Stored Uint16)))
                    -> (Ref s2 (Struct "userinput_decode_state"))
                    -> (Ref s3 (Struct "userinput_result"))
                    -> Uint32
                    -> Ivory s () ()
arming_statemachine pwms state out now = do
  ch5_switch <- deref (pwms ! (5 :: Ix Uint8 8))
  throttle_stick <- deref (pwms ! (2 :: Ix Uint8 8))
  rudder_stick   <- deref (pwms ! (3 :: Ix Uint8 8))

  ifte (ch5_switch <? 1500)<commit_msg>smaccmpilot- swap switch direction on mode switch to match pat's preference<commit_after>{-# LANGUAGE DataKinds #-}
{-# LANGUAGE TypeOperators #-}
{-# LANGUAGE FlexibleInstances #-}
{-# LANGUAGE QuasiQuotes #-}

module UserInputDecode where

import Ivory.Language

import IvoryHelpers

import qualified UserInputType as I

userInputDecodeModule :: Module
userInputDecodeModule = package "userinput_decode" $ do
  depend I.userInputModule
  defStruct (Proxy :: Proxy "userinput_decode_state")
  incl decode
  incl scale_proc

[ivory|
struct userinput_decode_state
  { last_modepwm      :: Stored Uint16
  ; last_modepwm_time :: Stored Uint32
  ; arm_state         :: Stored Uint8
  ; arm_state_time    :: Stored Uint32
  }
|]

as_DISARMED, as_ARMING, as_ARMED :: Uint8
as_DISARMED = 0
as_ARMING   = 1
as_ARMED    = 2

mode_STABILIZE, mode_ALT_HOLD, mode_LOITER :: Uint8
mode_STABILIZE = 0
mode_ALT_HOLD  = 1
mode_LOITER    = 2

mode_pwm_map :: [(Uint8, (Uint16, Uint16))]    -- on ER9X this should be:
mode_pwm_map = [(mode_LOITER,    (900, 1300))  -- AUX 3 up
               ,(mode_ALT_HOLD,  (1301, 1700)) -- AUX 3 center
               ,(mode_STABILIZE, (1701, 2100)) -- AUX 3 down
               ]

decode :: Def ('[ Ref s1 (Array 8 (Stored Uint16))
                , Ref s2 (Struct "userinput_decode_state")
                , Ref s3 (Struct "userinput_result")
                , Uint32 ] :-> ())
decode = proc "userinput_decode" $ \pwms state out now -> body $ do
  let chtransform ix f ofield = deref (pwms ! (ix :: Ix Uint8 8)) >>=
                                f >>= \v -> store (out ~> ofield) v
  store (out ~> I.time) now
  chtransform 0 scale_rpy I.roll
  chtransform 1 scale_rpy I.pitch
  chtransform 2 scale_thr I.throttle
  chtransform 3 scale_rpy I.yaw
  arming_statemachine pwms state out now
  mode_statemachine pwms state out now
  retVoid

arming_statemachine :: (Ref s1 (Array 8 (Stored Uint16)))
                    -> (Ref s2 (Struct "userinput_decode_state"))
                    -> (Ref s3 (Struct "userinput_result"))
                    -> Uint32
                    -> Ivory s () ()
arming_statemachine pwms state out now = do
  ch5_switch <- deref (pwms ! (5 :: Ix Uint8 8))
  throttle_stick <- deref (pwms ! (2 :: Ix Uint8 8))
  rudder_stick   <- deref (pwms ! (3 :: Ix Uint8 8))

  ifte (ch5_switch <? 1500)
{'fraction_non_alphanumeric': 0.10543693594541052, 'fraction_numerical': 0.03609949372661237, 'mean_word_length': 2.6823338735818476, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 29, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3381997', 'n_tokens_mistral': 1801, 'n_tokens_neox': 1628, 'n_words': 439}