stepper_controller/arduino/source/qepn.c

   1 /*****************************************************************************
   2 * Product: QEP-nano implemenation
   3 * Last Updated for Version: 4.0.03
   4 * Date of the Last Update:  Dec 26, 2008
   5 *
   6 *                    Q u a n t u m     L e a P s
   7 *                    ---------------------------
   8 *                    innovating embedded systems
   9 *
  10 * Copyright (C) 2002-2008 Quantum Leaps, LLC. All rights reserved.
  11 *
  12 * This software may be distributed and modified under the terms of the GNU
  13 * General Public License version 2 (GPL) as published by the Free Software
  14 * Foundation and appearing in the file GPL.TXT included in the packaging of
  15 * this file. Please note that GPL Section 2[b] requires that all works based
  16 * on this software must also be made publicly available under the terms of
  17 * the GPL ("Copyleft").
  18 *
  19 * Alternatively, this software may be distributed and modified under the
  20 * terms of Quantum Leaps commercial licenses, which expressly supersede
  21 * the GPL and are specifically designed for licensees interested in
  22 * retaining the proprietary status of their code.
  23 *
  24 * Contact information:
  25 * Quantum Leaps Web site:  http://www.quantum-leaps.com
  26 * e-mail:                  [email protected]
  27 *****************************************************************************/
  28 #include "qpn_port.h"                                       /* QP-nano port */
  29
  30 #ifndef Q_NHSM
  31 Q_DEFINE_THIS_MODULE(qepn)
  32 #endif
  33
  34 /**
  35 * \file
  36 * \ingroup qepn qfn
  37 * QEP-nano implementation.
  38 */
  39
  40 /** empty signal for internal use only */
  41 #define QEP_EMPTY_SIG_        0
  42
  43 /** maximum depth of state nesting (including the top level), must be >= 2 */
  44 #define QEP_MAX_NEST_DEPTH_   5
  45
  46 /*..........................................................................*/
  47 /*lint -e970 -e971 */      /* ignore MISRA rules 13 and 14 in this function */
  48 char const Q_ROM * Q_ROM_VAR QP_getVersion(void) {
  49     static char const Q_ROM Q_ROM_VAR version[] = "4.0.03";
  50     return version;
  51 }
  52
  53 #ifndef Q_NFSM
  54 /*..........................................................................*/
  55 void QFsm_init(QFsm *me) {
  56     (void)(*me->state)(me);      /* execute the top-most initial transition */
  57
  58     Q_SIG(me) = (QSignal)Q_ENTRY_SIG;
  59     (void)(*me->state)(me);                             /* enter the target */
  60 }
  61 /*..........................................................................*/
  62 #ifndef QK_PREEMPTIVE
  63 void QFsm_dispatch(QFsm *me) {
  64 #else
  65 void QFsm_dispatch(QFsm *me) Q_REENTRANT {
  66 #endif
  67     QStateHandler s = me->state;
  68
  69     if ((*s)(me) == Q_RET_TRAN) {                      /* transition taken? */
  70         Q_SIG(me) = (QSignal)Q_EXIT_SIG;
  71         (void)(*s)(me);                                  /* exit the source */
  72
  73         Q_SIG(me) = (QSignal)Q_ENTRY_SIG;
  74         (void)(*me->state)(me);                         /* enter the target */
  75     }
  76 }
  77 #endif                                                            /* Q_NFSM */
  78
  79 #ifndef Q_NHSM
  80 /*..........................................................................*/
  81 QState QHsm_top(QHsm *me) {
  82     (void)me;             /* supress the "unused argument" compiler warning */
  83     return Q_IGNORED();                 /* the top state ignores all events */
  84 }
  85 /*..........................................................................*/
  86 void QHsm_init(QHsm *me) {
  87     QStateHandler t;
  88
  89     Q_ALLEGE((*me->state)(me) == Q_RET_TRAN);/* initial tran. must be taken */
  90
  91     t = (QStateHandler)&QHsm_top;         /* an HSM starts in the top state */
  92     do {                              /* drill into the target hierarchy... */
  93         QStateHandler path[QEP_MAX_NEST_DEPTH_];
  94         int8_t ip = (int8_t)0;
  95
  96         path[0] = me->state;
  97         Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
  98         (void)(*me->state)(me);
  99         while (me->state != t) {
 100             path[++ip] = me->state;
 101             (void)(*me->state)(me);
 102         }
 103         me->state = path[0];
 104                                             /* entry path must not overflow */
 105         Q_ASSERT(ip < (int8_t)QEP_MAX_NEST_DEPTH_);
 106
 107         Q_SIG(me) = (QSignal)Q_ENTRY_SIG;
 108         do {        /* retrace the entry path in reverse (correct) order... */
 109             (void)(*path[ip])(me);                        /* enter path[ip] */
 110         } while ((--ip) >= (int8_t)0);
 111
 112         t = path[0];
 113         Q_SIG(me) = (QSignal)Q_INIT_SIG;
 114     } while ((*t)(me) == Q_RET_TRAN);        /* initial transition handled? */
 115     me->state = t;
 116 }
 117 /*..........................................................................*/
 118 #ifndef QK_PREEMPTIVE
 119 void QHsm_dispatch(QHsm *me) {
 120 #else
 121 void QHsm_dispatch(QHsm *me) Q_REENTRANT {
 122 #endif
 123     QStateHandler path[QEP_MAX_NEST_DEPTH_];
 124     QStateHandler s;
 125     QStateHandler t;
 126     QState r;
 127
 128     t = me->state;                                /* save the current state */
 129
 130     do {                             /* process the event hierarchically... */
 131         s = me->state;
 132         r = (*s)(me);                             /* invoke state handler s */
 133     } while (r == Q_RET_SUPER);
 134
 135     if (r == Q_RET_TRAN) {                             /* transition taken? */
 136         int8_t ip = (int8_t)(-1);            /* transition entry path index */
 137         int8_t iq;                    /* helper transition entry path index */
 138
 139         path[0] = me->state;           /* save the target of the transition */
 140         path[1] = t;
 141
 142         while (t != s) {    /* exit current state to transition source s... */
 143             Q_SIG(me) = (QSignal)Q_EXIT_SIG;        /* find superstate of t */
 144             if ((*t)(me) == Q_RET_HANDLED) {        /* exit action handled? */
 145                 Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 146                 (void)(*t)(me);                     /* find superstate of t */
 147             }
 148             t = me->state;                /* me->state holds the superstate */
 149         }
 150
 151         t = path[0];                            /* target of the transition */
 152
 153         if (s == t) {      /* (a) check source==target (transition to self) */
 154             Q_SIG(me) = (QSignal)Q_EXIT_SIG;
 155             (void)(*s)(me);                              /* exit the source */
 156             ip = (int8_t)0;                             /* enter the target */
 157         }
 158         else {
 159             Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 160             (void)(*t)(me);                    /* find superstate of target */
 161             t = me->state;
 162             if (s == t) {                /* (b) check source==target->super */
 163                 ip = (int8_t)0;                         /* enter the target */
 164             }
 165             else {
 166                 Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 167                 (void)(*s)(me);                /* find superstate of source */
 168
 169                                   /* (c) check source->super==target->super */
 170                 if (me->state == t) {
 171                     Q_SIG(me) = (QSignal)Q_EXIT_SIG;
 172                     (void)(*s)(me);                      /* exit the source */
 173                     ip = (int8_t)0;                     /* enter the target */
 174                 }
 175                 else {
 176                                          /* (d) check source->super==target */
 177                     if (me->state == path[0]) {
 178                         Q_SIG(me) = (QSignal)Q_EXIT_SIG;
 179                         (void)(*s)(me);                  /* exit the source */
 180                     }
 181                     else { /* (e) check rest of source==target->super->super..
 182                             * and store the entry path along the way
 183                             */
 184                         iq = (int8_t)0;      /* indicate that LCA not found */
 185                         ip = (int8_t)1;  /* enter target and its superstate */
 186                         path[1] = t;       /* save the superstate of target */
 187                         t = me->state;                /* save source->super */
 188
 189                         Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 190                         r = (*path[1])(me);    /* find target->super->super */
 191                         while (r == Q_RET_SUPER) {
 192                             path[++ip] = me->state; /* store the entry path */
 193                             if (me->state == s) {      /* is it the source? */
 194                                 iq = (int8_t)1;  /* indicate that LCA found */
 195                                             /* entry path must not overflow */
 196                                 Q_ASSERT(ip < (int8_t)QEP_MAX_NEST_DEPTH_);
 197                                 --ip;            /* do not enter the source */
 198                                 r = Q_RET_HANDLED;    /* terminate the loop */
 199                             }
 200                             else {   /* it is not the source, keep going up */
 201                                 r = (*me->state)(me);    /* superstate of t */
 202                             }
 203                         }
 204                         if (iq == (int8_t)0) {    /* the LCA not found yet? */
 205
 206                                             /* entry path must not overflow */
 207                             Q_ASSERT(ip < (int8_t)QEP_MAX_NEST_DEPTH_);
 208
 209                             Q_SIG(me) = (QSignal)Q_EXIT_SIG;
 210                             (void)(*s)(me);              /* exit the source */
 211
 212                                 /* (f) check the rest of source->super
 213                                  *                  == target->super->super...
 214                                  */
 215                             iq = ip;
 216                             r = Q_RET_IGNORED;    /* indicate LCA NOT found */
 217                             do {
 218                                 s = path[iq];
 219                                 if (t == s) {           /* is this the LCA? */
 220                                     r = Q_RET_HANDLED;/* indicate LCA found */
 221                                     ip = (int8_t)(iq - 1);/*do not enter LCA*/
 222                                     iq = (int8_t)(-1);/* terminate the loop */
 223                                 }
 224                                 else {
 225                                     --iq; /* try lower superstate of target */
 226                                 }
 227                             } while (iq >= (int8_t)0);
 228
 229                             if (r != Q_RET_HANDLED) { /* LCA not found yet? */
 230                                     /* (g) check each source->super->...
 231                                      * for each target->super...
 232                                      */
 233                                 r = Q_RET_IGNORED;          /* keep looping */
 234                                 do {
 235                                                        /* exit t unhandled? */
 236                                     Q_SIG(me) = (QSignal)Q_EXIT_SIG;
 237                                     if ((*t)(me) == Q_RET_HANDLED) {
 238                                         Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 239                                         (void)(*t)(me);  /* find super of t */
 240                                     }
 241                                     t = me->state;    /*  set to super of t */
 242                                     iq = ip;
 243                                     do {
 244                                         s = path[iq];
 245                                         if (t == s) {       /* is this LCA? */
 246                                                         /* do not enter LCA */
 247                                             ip = (int8_t)(iq - 1);
 248                                             iq = (int8_t)(-1);/*break inner */
 249                                             r = Q_RET_HANDLED;/*break outer */
 250                                         }
 251                                         else {
 252                                             --iq;
 253                                         }
 254                                     } while (iq >= (int8_t)0);
 255                                 } while (r != Q_RET_HANDLED);
 256                             }
 257                         }
 258                     }
 259                 }
 260             }
 261         }
 262                     /* retrace the entry path in reverse (desired) order... */
 263         Q_SIG(me) = (QSignal)Q_ENTRY_SIG;
 264         for (; ip >= (int8_t)0; --ip) {
 265             (void)(*path[ip])(me);                        /* enter path[ip] */
 266         }
 267         t = path[0];                      /* stick the target into register */
 268         me->state = t;                          /* update the current state */
 269
 270                                       /* drill into the target hierarchy... */
 271         Q_SIG(me) = (QSignal)Q_INIT_SIG;
 272         while ((*t)(me) == Q_RET_TRAN) {
 273             ip = (int8_t)0;
 274
 275             path[0] = me->state;
 276             Q_SIG(me) = (QSignal)QEP_EMPTY_SIG_;
 277             (void)(*me->state)(me);                  /* find the superstate */
 278             while (me->state != t) {
 279                 path[++ip] = me->state;
 280                 (void)(*me->state)(me);              /* find the superstate */
 281             }
 282             me->state = path[0];
 283                                             /* entry path must not overflow */
 284             Q_ASSERT(ip < (int8_t)QEP_MAX_NEST_DEPTH_);
 285
 286             Q_SIG(me) = (QSignal)Q_ENTRY_SIG;
 287             do {    /* retrace the entry path in reverse (correct) order... */
 288                 (void)(*path[ip])(me);                    /* enter path[ip] */
 289             } while ((--ip) >= (int8_t)0);
 290
 291             t = path[0];
 292             Q_SIG(me) = (QSignal)Q_INIT_SIG;
 293         }
 294     }
 295     me->state = t;            /* set new state or restore the current state */
 296 }
 297
 298 #endif                                                            /* Q_NHSM */