hostside/actual.c

   1 /*
   2  * realtime
   3  * implementation of safety algorithm's decisions
   4  */
   5
   6 #include "realtime.h"
   7
   8 static PicInsn polarityinsn= { { 0x90 }, 0 };
   9
  10 #define OPONBIT_BYTE (polarityinsn.d[bytenum])
  11 #define OPONBIT(body) do{                               \
  12     BoardObject bo;                                     \
  13     int bytenum, bitnum;                                \
  14     Byte bitv;                                          \
  15                                                         \
  16     bo= segi->invert;                                   \
  17     assert(bo);                                         \
  18     bytenum= (bo + 3) / 7;                              \
  19     bitnum= bytenum ? 6 - (bo + 3) % 7 : 3 - bo;        \
  20     bitv= 1u << bitnum;                                 \
  21                                                         \
  22     { body }                                            \
  23   }while(0)
  24
  25 int picinsn_polarity_testbit(const PicInsn *pi, const SegmentInfo *segi) {
  26   OPONBIT({
  27     return !!(OPONBIT_BYTE & bitv);
  28   });
  29 }
  30
  31 void actual_inversions_start(void) {
  32   int i;
  33   polarityinsn.l= (info_maxreverse + 4 + 6) / 7;
  34   for (i=1; i<polarityinsn.l-1; i++)
  35     polarityinsn.d[i] |= 0x80;
  36 }
  37
  38 void actual_inversions_segment(Segment *seg) {
  39   const SegmentInfo *segi= seg->i;
  40
  41   OPONBIT({
  42     Byte *insnbyte= &OPONBIT_BYTE;
  43     if (seg->seg_inverted) *insnbyte |= bitv;
  44     else *insnbyte &= ~bitv;
  45   });
  46 }
  47
  48 void actual_inversions_done(void) {
  49   serial_transmit(&polarityinsn);
  50 }