int/nucleo-f767-blinky/src/sds.c

   1 #include <stdlib.h>
   2 #include <string.h>
   3
   4 #ifndef STM32F767xx
   5 #include <unistd.h>
   6 #include <stdio.h>
   7 #endif
   8
   9 #include "interface.h"
  10 #include "interpret.h"
  11 #include "misc.h"
  12 #include "sds.h"
  13
  14 struct sds sdss[MAXSDSS];
  15 uint8_t c;
  16
  17 void sds_init()
  18 {
  19         memset(&sdss, 0, sizeof(struct sds)*MAXSDSS);
  20 }
  21
  22 void sds_register()
  23 {
  24         uint8_t cs;
  25         for(cs = 0; cs<MAXSDSS; cs++)
  26                 if(!sdss[cs].used)
  27                         break;
  28
  29         if(cs == MAXSDSS)
  30                 die("Trying to add too much tasks...\n");
  31
  32         memset(&sdss[cs], 0, sizeof(struct sds));
  33         //Read identifier
  34         sdss[cs].id = read16();
  35         //Read value
  36         sdss[cs].value = read16();
  37
  38         debug("\nReceived sds %d: %d\n", sdss[cs].id, sdss[cs].value);
  39         sdss[cs].used = true;
  40 }
  41
  42 bool sds_update()
  43 {
  44         uint8_t cs, id;
  45         //Read identifier
  46         id = read16();
  47
  48         for(cs = 0; cs<MAXSDSS; cs++){
  49                 if(!sdss[cs].used)
  50                         continue;
  51                 if(sdss[cs].id == id){
  52                         //Read value
  53                         sdss[cs].value = read16();
  54                         debug("\nReceived sds update %d: %d\n",
  55                               sdss[cs].id, sdss[cs].value);
  56                         return true;
  57                 }
  58         }
  59         return false;
  60 }
  61
  62 void sds_publish(int id)
  63 {
  64         uint8_t cs;
  65         for(cs = 0; cs<MAXSDSS; cs++){
  66                 if(sdss[cs].used && sdss[cs].id == id){
  67                         debug("Publish %d=%d\n", sdss[cs].id, sdss[cs].value);
  68                         write_byte('u');
  69                         write_byte(0);
  70                         write_byte(sdss[cs].id);
  71                         write_byte(sdss[cs].value/265);
  72                         write_byte(sdss[cs].value%265);
  73                         return;
  74                 }
  75         }
  76         debug("SDS identifier unknown: %d\n", id);
  77 }
  78
  79 int sds_fetch(int id)
  80 {
  81         uint8_t cs;
  82         for(cs = 0; cs<MAXSDSS; cs++)
  83                 if(sdss[cs].used && sdss[cs].id == id)
  84                         return sdss[cs].value;
  85         debug("SDS identifier unknown: %d\n", id);
  86         return 0;
  87 }
  88
  89 void sds_store(int id, int val)
  90 {
  91         uint8_t cs;
  92         for(cs = 0; cs<MAXSDSS; cs++) {
  93                 if(sdss[cs].used && sdss[cs].id == id){
  94                         sdss[cs].value = val;
  95                         return;
  96                 }
  97         }
  98         debug("SDS identifier unknown: %d\n", id);
  99 }