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