stmbl

HAL_COMP(term);

HAL_PIN(wave0) = 0.0;
HAL_PIN(gain0) = 10.0;
HAL_PIN(offset0) = 0.0;

HAL_PIN(wave1) = 0.0;
HAL_PIN(gain1) = 10.0;
HAL_PIN(offset1) = 0.0;

HAL_PIN(wave2) = 0.0;
HAL_PIN(gain2) = 10.0;
HAL_PIN(offset2) = 0.0;

HAL_PIN(wave3) = 0.0;
HAL_PIN(gain3) = 10.0;
HAL_PIN(offset3) = 0.0;

HAL_PIN(wave4) = 0.0;
HAL_PIN(gain4) = 10.0;
HAL_PIN(offset4) = 0.0;

HAL_PIN(wave5) = 0.0;
HAL_PIN(gain5) = 10.0;
HAL_PIN(offset5) = 0.0;

HAL_PIN(wave6) = 0.0;
HAL_PIN(gain6) = 10.0;
HAL_PIN(offset6) = 0.0;

HAL_PIN(wave7) = 0.0;
HAL_PIN(gain7) = 10.0;
HAL_PIN(offset7) = 0.0;

HAL_PIN(send_step) = 10.0;

HAL_PIN(buf_use) = 0.0;
HAL_PIN(tx_buf_use) = 0.0;

HAL_PIN(fault) = 0.0;
HAL_PIN(con) = 0.0;

HAL_PIN(tim2_rv) = 900.0;

#define TERM_BUF_SIZE 24

MEM(float w0[TERM_BUF_SIZE]);
MEM(float w1[TERM_BUF_SIZE]);
MEM(float w2[TERM_BUF_SIZE]);
MEM(float w3[TERM_BUF_SIZE]);
MEM(float w4[TERM_BUF_SIZE]);
MEM(float w5[TERM_BUF_SIZE]);
MEM(float w6[TERM_BUF_SIZE]);
MEM(float w7[TERM_BUF_SIZE]);

MEM(unsigned int read_pos) = 0;
MEM(unsigned int write_pos) = 0;
MEM(unsigned int send_counter) = 0;

MEM(unsigned int last_fault_sent) = 0; //save last error message
MEM(uint32_t last_hal_state) = HAL_OK;
MEM(uint16_t ee_error);

/*
int load(){
   if(hal.rt_state != RT_STOP || hal.frt_state != FRT_STOP){
      return(-4);
   }
   if(ee_error != FLASH_COMPLETE){
      return -1;
   }
   typedef union{
      float f;
      uint16_t byte[2];
   }param_t;
   param_t param;
   uint16_t address = 0;
   uint16_t lo;
   uint16_t hi;
   uint16_t elo;
   uint16_t ehi;
   for(int i = 0; i < hal.hal_pin_count; i++){
      if(address >= NB_OF_VAR){
         return -2;
      }
      char name[6];
      strncpy(name,hal.hal_pins[i]->name,5);
      name[5] =  '\0';
      if(!strcmp(name, "conf0")){
         elo = EE_ReadVariable(address,&lo);
         ehi = EE_ReadVariable(address+1,&hi);
         if(elo == 0 && ehi == 0){
            param.byte[0] = lo;
            param.byte[1] = hi;
            hal.hal_pins[i]->value = param.f;
         }else{
            return -3;
         }
         address+=2;
         Wait(1);
      }
   }
   //update cmd/fb links
   update_cmd();
   update_fb();
   return 0;
}
*/

INIT(
   /*
   FLASH_Unlock();
   ee_error = EE_Init();
   FLASH_Lock();
   int ret = load();
   if(ret){//flash load error
      hal.hal_state = CONFIG_LOAD_ERROR;
   }
   */
);

RT(
   timer_set_period(TIM2, (int)CLAMP(PIN(tim2_rv), 20.0, 60000.0));
   PIN(tim2_rv) = (int)CLAMP(PIN(tim2_rv), 20.0, 60000.0);
   if(send_counter++ >= PIN(send_step) - 1){
      w0[write_pos] = PIN(wave0);
      w1[write_pos] = PIN(wave1);
      w2[write_pos] = PIN(wave2);
      w3[write_pos] = PIN(wave3);
      w4[write_pos] = PIN(wave4);
      w5[write_pos] = PIN(wave5);
      w6[write_pos] = PIN(wave6);
      w7[write_pos] = PIN(wave7);

      write_pos++;
      write_pos %= TERM_BUF_SIZE;
      send_counter = 0;
   }
);

NRT(

   void bootloader(){
      *((unsigned long *)0x2001C000) = 0xDEADBEEF;
      scb_reset_system();
   }

   void reset(){
      scb_reset_system();
   }

   void hal_info(){
      printf("######## hal info ########\n");
      printf("#pins %i\n", hal.hal_pin_count);
      printf("#comps %i\n", hal.comp_count);
      printf("link errors %lu\n", hal.link_errors);
      printf("pin errors %lu\n", hal.pin_errors);
      printf("comp errors %lu\n", hal.comp_errors);
      printf("set errors %lu\n", hal.set_errors);
      printf("get errors %lu\n", hal.get_errors);
      printf("foo0.bar:  %f\n", get_hal_pin("foo0.bar"));
      printf("error_name: %s\n",hal.error_name);
      float pe = get_hal_pin("net0.rt_period");
      float ct = get_hal_pin("net0.rt_calc_time");
      if(pe > 0.0){
         printf("rt freq: %fHz\n", 1.0 / pe);
         printf("rt time: %f/%fs", ct, pe);
         printf("=%f%%\n",(ct/pe)*100);
      }
      pe = get_hal_pin("net0.frt_period");
      ct = get_hal_pin("net0.frt_calc_time");
      if(pe > 0.0){
         printf("frt freq: %fHz\n", 1.0 / pe);
         printf("frt time: %f/%fs", ct, pe);
         printf("=%f%%\n",(ct/pe)*100);
      }
      pe = get_hal_pin("net0.nrt_period");
      ct = get_hal_pin("net0.nrt_calc_time");
      if(pe > 0.0){
         printf("nrt time: %f/%fs", ct, pe);
         printf("=%f%%\n",(ct/pe)*100);
      }
      switch(hal.rt_state){
         case RT_STOP:
            printf("rt state:  STOP\n");
            break;
         case RT_SLEEP:
            printf("rt state:  SLEEP\n");
            break;
         case RT_CALC:
            printf("rt state:  CALC\n");
            break;
      }
      switch(hal.frt_state){
         case FRT_STOP:
            printf("frt state:  STOP\n");
            break;
         case FRT_SLEEP:
            printf("frt state:  SLEEP\n");
            break;
         case FRT_CALC:
            printf("frt state:  CALC\n");
            break;
      }
      switch(hal.hal_state){
         case HAL_OK:
            printf("HAL state:  HAL_OK\n");
            break;
         case RT_TOO_LONG:
            printf("HAL state:  RT_TOO_LONG\n");
            break;
         case FRT_TOO_LONG:
            printf("HAL state:  FRT_TOO_LONG\n");
            break;
         case MISC_ERROR:
            printf("HAL state:  MISC_ERROR\n");
            break;
         case MEM_ERROR:
            printf("HAL state:  MEM_ERROR\n");
            break;
         case CONFIG_LOAD_ERROR:
            printf("HAL state:  CONFIG_LOAD_ERROR\n");
            break;
         case CONFIG_ERROR:
            printf("HAL state:  CONFIG_ERROR\n");
            break;
      }
      uint32_t p = 0;
      printf("active rt funcs(%u):\n", hal.rt_func_count);
      for(int i = 0; i < hal.rt_func_count; i++){
         p = (uint32_t)hal.rt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt){
               printf("   %s%lu.rt(%f)\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance,  hal.hal_pins[hal.hal_comps[i]->hal_pin_start_index + 2]->source->source->value);
               break;
            }
         }
      }
      printf("\nactive frt funcs(%u):\n", hal.frt_func_count);
      for(int i = 0; i < hal.frt_func_count; i++){
         p = (uint32_t)hal.frt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->frt){
               printf("   %s%lu.frt(%f)\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance, hal.hal_pins[hal.hal_comps[i]->hal_pin_start_index + 3]->source->source->value);
               break;
            }
         }
      }
      printf("\nactive rt_init funcs(%u):\n", hal.rt_init_func_count);
      for(int i = 0; i < hal.rt_init_func_count; i++){
         p = (uint32_t)hal.rt_init[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt_init){
               printf("   %s%lu.rt_init\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf("\nactive rt_deinit funcs(%u):\n", hal.rt_deinit_func_count);
      for(int i = 0; i < hal.rt_deinit_func_count; i++){
         p = (uint32_t)hal.rt_deinit[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt_deinit){
               printf("   %s%lu.rt_deinit\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf("\nactive nrt_init funcs(%u):\n", hal.nrt_init_func_count);
      for(int i = 0; i < hal.nrt_init_func_count; i++){
         p = (uint32_t)hal.nrt_init[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->nrt_init){
               printf("   %s%lu.nrt_init\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf("\nactive nrt funcs(%u):\n", hal.nrt_func_count);
      for(int i = 0; i < hal.nrt_func_count; i++){
         p = (uint32_t)hal.nrt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->nrt){
               printf("   %s%lu.nrt\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf("\n");
   }

   void about(){
      printf("######## software info ########\n");
      printf(
          "%s v%i.%i.%i %s\n",
          version_info.product_name,
          version_info.major,
          version_info.minor,
          version_info.patch,
          version_info.git_version
      );
      printf("Branch %s\n",version_info.git_branch);
      printf("Compiled %s %s ",version_info.build_date, version_info.build_time);
      printf("by %s on %s\n",version_info.build_user, version_info.build_host);
      printf("GCC        %s\n",__VERSION__);
      printf("newlib     %s\n",_NEWLIB_VERSION);
      printf("CPU ID     %lx %lx %lx\n",DESIG_UNIQUE_ID0, DESIG_UNIQUE_ID1, DESIG_UNIQUE_ID2);
      crc_reset();
      uint32_t crc = crc_calculate_block((uint32_t *) &vector_table, version_info.image_size / 4);
      printf("size: %lu crc:%lx\n", version_info.image_size, version_info.image_crc);
      if(crc == 0)
        printf("crc ok!\n");
      else
        printf("crc error!:%lx\n", crc);
      
      printf("######## Bootloader info ########\n");
      printf(
          "%s v%i.%i.%i %s\n",
          bt_version_info->product_name,
          bt_version_info->major,
          bt_version_info->minor,
          bt_version_info->patch,
          bt_version_info->git_version
      );
      printf("Branch %s\n",bt_version_info->git_branch);
      printf("Compiled %s %s ",bt_version_info->build_date, bt_version_info->build_time);
      printf("by %s on %s\n",bt_version_info->build_user, bt_version_info->build_host);

      //printf("start:%p ,size:%p ,end%p \n",&_binary_obj_hv_hv_bin_start,&_binary_obj_hv_hv_bin_size,&_binary_obj_hv_hv_bin_end);
   }

   void start(){
      printf("starting hal ... ");
      start_hal();
      printf("done\n");
   }

   void stop(){
      printf("stopping hal ... ");
      stop_hal();
      printf("done\n");
   }

   void sysinfo(){
      printf("######## sysinfo ########\n");
      // extern char _etext;    // end address of the .text section
      // extern char _sidata;   // start address of the initialization values of the .data section
      // extern char _sdata;    // start address of the .data section
      // extern char _edata;    // end address of the .data section
      // extern char _sbss;     // start address of the .bss section
      // extern char _ebss;     // end address of the .bss section
      //extern char _snoinit;  // start address of the .noinit section
      //extern char _enoinit;  // end address of the .noinit section
      // extern char _end;      // end address of the .bss section
      // extern char _estack;   // initial value of the stack pointer
      //
      // extern char *__brkval;
      
      printf("rcc_ahb_frequency  = %luHz\n", rcc_ahb_frequency);
      printf("rcc_apb1_frequency = %luHz\n", rcc_apb1_frequency);
      printf("rcc_apb2_frequency = %luHz\n", rcc_apb2_frequency);
      printf("vector_table = 0x%lx\n",(uint32_t)&vector_table);
      printf("SCB_VTOR = 0x%lx\n", SCB_VTOR);
      printf("RCC_CSR = 0x%lx\n", RCC_CSR);
      // printf("_etext   = %p\n", &_etext  );
      // printf("_sidata  = %p\n", &_sidata );
      // printf("_sdata   = %p\n", &_sdata  );
      // printf("_edata   = %p\n", &_edata  );
      // printf("_sbss    = %p\n", &_sbss   );
      // printf("_ebss    = %p\n", &_ebss   );
      // printf("_snoinit = %p\n", &_snoinit);
      // printf("_enoinit = %p\n", &_enoinit);
      // printf("_end     = %p\n", &_end    );
      // printf("_estack  = %p\n", &_estack );

      // printf("heap avail       = %luB\n", (uint32_t)((char*)__get_MSP() - __brkval));
   }

   void listhal(){
      for(int i = 0; i < hal.hal_pin_count; i++){
         printf("%s <= %s = %f\n", hal.hal_pins[i]->name, hal.hal_pins[i]->source->name, hal.hal_pins[i]->source->source->value);
         //Wait(1);
      }
   }
   /*
   void save(){
      if(ee_error != FLASH_COMPLETE){
         printf("flash error:%i\n",ee_error);
         return;
      }
      typedef union{
         float f;
         uint16_t byte[2];
      }param_t;
      param_t param;
      uint16_t elo;
      uint16_t ehi;
      uint16_t address = 0;
      FLASH_Unlock();
      for(int i = 0; i < hal.hal_pin_count; i++){
         if(address >= NB_OF_VAR){
            printf("NB_OF_VAR too small\n");
            FLASH_Lock();
            return;
         }
         char name[6];
         strncpy(name,hal.hal_pins[i]->name,5);
         name[5] =  '\0';
         if(!strcmp(name, "conf0")){
            param.f = hal.hal_pins[i]->source->source->value;
            //printf("param: %s=%f address:%i\n",hal.hal_pins[i]->name,param.f,address);
            elo = EE_WriteVariable(address,param.byte[0]);
            ehi = EE_WriteVariable(address+1,param.byte[1]);
            if(elo != FLASH_COMPLETE || ehi != FLASH_COMPLETE){
               printf("error writing to %i: error%i,%i\n",address,elo,ehi);
               break;
            }
            address+=2;
         }
      }
      FLASH_Lock();
      printf("done\n");
   }
   */

   void conf(){
      for(int i = 0; i < hal.hal_pin_count; i++){
         char name[6];
         strncpy(name,hal.hal_pins[i]->name,5);
         name[5] =  '\0';
         if(!strcmp(name, "conf0")){
            printf("%s = %f\n", hal.hal_pins[i]->name, hal.hal_pins[i]->value);
         }
      }
   }
   /*
   void load_cmd(){
      int ret = load();
      if(ret == -1){
         printf("flash error:%i\n",ee_error);
         return;
      }else if(ret == -2){
         printf("NB_OF_VAR too small\n");
         return;
      }else if(ret == -3){
         printf("flash read error\n");
      }else if(ret == -4){
         printf("hal running error\n");
      }else{
         printf("done\n");
      }
   }
*/
   void help(){
      printf("######## HAL cheat sheet ########\n");
      printf("pin name: <comp name><comp instance number>.<pin name> (e.g. pid0.enable)\n");
      printf("show pin value and source: pid0.enable (answer: pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf("set pin: pid0.enable = 1.5 (answer: OK pid0.enable = 1.500000)\n");
      printf("link pin: pid0.enable = fault0.enable_pid (answer: OK pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf("unlink pin: pid0.enable = pid0.enable (answer: OK pid0.enable <= pid0.enable = 1.000000)\n");
      printf("show hal info: hal\n");
      printf("show sys info: sysinfo\n");
      printf("show software version: about\n");
      printf("stop realtime hal: stop\n");
      printf("start realtime hal: start\n");
      printf("change rt priority: pid0.rt_prio = 6 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf("change frt priority: pid0.frt_prio = -1 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf("reset cpu: reset\n");
      printf("print help: help\n");
      printf("list all hal pins: list\n");
      printf("list all conf pins: conf\n");
      printf("save conf: save\n");
      printf("load conf: load\n");
   }

   void cmd(char * s){
      if(!strcmp(s, "bootloader")){
         bootloader();
      }
      else if(!strcmp(s, "start")){
         start();
      }
      else if(!strcmp(s, "stop")){
         stop();
      }
      else if(!strcmp(s, "hal")){
         hal_info();
      }
      else if(!strcmp(s, "about")){
         about();
      }
      else if(!strcmp(s, "sysinfo")){
         sysinfo();
      }
      else if(!strcmp(s, "reset")){
         reset();
      }
      else if(!strcmp(s, "list")){
         listhal();
      }
      else if(!strcmp(s, "help")){
         help();
      }
      else if(!strcmp(s, "save")){
         //TODO:
         //save();
         printf("not implemented\n");
      }
      else if(!strcmp(s, "load")){
         //TODO:
         //load_cmd();
         printf("not implemented\n");
      }
      else if(!strcmp(s, "conf")){
         conf();
      }
      else{
         printf("cmd not found: %s\n",s);
      }
   }

   void hal_error_sender(){
      if(hal.hal_state != last_hal_state){
         last_hal_state = hal.hal_state;
         switch(hal.hal_state){
            case HAL_OK:
               printf("HAL state:  HAL_OK\n");
               break;
            case RT_TOO_LONG:
               printf("HAL state:  RT_TOO_LONG\n");
               break;
            case FRT_TOO_LONG:
               printf("HAL state:  FRT_TOO_LONG\n");
               break;
            case MISC_ERROR:
               printf("HAL state:  MISC_ERROR\n");
               break;
            case MEM_ERROR:
               printf("HAL state:  MEM_ERROR\n");
               break;
            case CONFIG_LOAD_ERROR:
               printf("HAL state:  CONFIG_LOAD_ERROR\n");
               break;
            case CONFIG_ERROR:
               printf("HAL state:  CONFIG_ERROR\n");
               break;
         }
      }
   }

   void fault_sender(){

     int flt = (int)PIN(fault);

     if(flt != last_fault_sent){
       last_fault_sent = flt;
       switch(flt){
         case STATE_DISABLED:
           printf("INFO: Disabled \n");
         break;

         case STATE_RESET:
           printf("INFO: Reset \n");
         break;

         case STATE_FB_ERROR:
           printf("ERROR: Feedback \n");
         break;

         case STATE_SAT_ERROR:
           printf("ERROR: Saturation \n");
         break;

         case STATE_OVR_CURR:
           printf("ERROR: Overcurrent \n");
         break;

         case STATE_POS_ERROR:
           printf("ERROR: Position \n");
         break;

         case STATE_OVR_TEMP:
           printf("ERROR: Overtemperture \n");
         break;

         case STATE_OVR_VOLT:
           printf("ERROR: Overvoltage \n");
         break;

         case STATE_ENABLED:
           printf("INFO: Enabled \n");
         break;

         case STATE_PHASING:
           printf("INFO: Phasing \n");
         break;

         default:
         break;
       }
     }
   }


   int e = 0;
   unsigned char buf[8 + 2];

   buf[0] = 255;
   buf[8 + 1] = 0;

   float o0 = PIN(offset0);
   float o1 = PIN(offset1);
   float o2 = PIN(offset2);
   float o3 = PIN(offset3);
   float o4 = PIN(offset4);
   float o5 = PIN(offset5);
   float o6 = PIN(offset6);
   float o7 = PIN(offset7);
   float g0 = PIN(gain0);
   float g1 = PIN(gain1);
   float g2 = PIN(gain2);
   float g3 = PIN(gain3);
   float g4 = PIN(gain4);
   float g5 = PIN(gain5);
   float g6 = PIN(gain6);
   float g7 = PIN(gain7);


   unsigned int wp = write_pos;
   unsigned int bc = 0;

   while(read_pos != wp){
      bc++;
      e = (w0[read_pos] + o0) * g0 + 128;
      buf[1] = CLAMP(e,1,254);

      e = (w1[read_pos] + o1) * g1 + 128;
      buf[2] = CLAMP(e,1,254);

      e = (w2[read_pos] + o2) * g2 + 128;
      buf[3] = CLAMP(e,1,254);

      e = (w3[read_pos] + o3) * g3 + 128;
      buf[4] = CLAMP(e,1,254);

      e = (w4[read_pos] + o4) * g4 + 128;
      buf[5] = CLAMP(e,1,254);

      e = (w5[read_pos] + o5) * g5 + 128;
      buf[6] = CLAMP(e,1,254);

      e = (w6[read_pos] + o6) * g6 + 128;
      buf[7] = CLAMP(e,1,254);

      e = (w7[read_pos] + o7) * g7 + 128;
      buf[8] = CLAMP(e,1,254);

      read_pos++;
      read_pos %= TERM_BUF_SIZE;

      if (cdcacm_is_connected()) {
         PIN(con) = 1.0;
         //cdcacm_tx(buf,9);
      }else{
         PIN(con) = 0.0;
      }
   }

   PIN(buf_use) = bc;
   PIN(tx_buf_use) = tx_buf.len;



   if (cdcacm_is_connected()) {
      char source[MAX_HPNAME];
      char sink[MAX_HPNAME];

      fault_sender();
      hal_error_sender();

      int i = -1;
      char rx_buf2[129];
      if (cdcacm_getline(rx_buf2, 129)){
         i = sscanf_(rx_buf2, "%N = %N", sink, source);
      }
      if(i == 2){ // read hal pin
         if(is_hal_pin(sink)){
            printf("%s <= %s = %f\n", sink, find_hal_pin(sink)->source->name, get_hal_pin(sink));
         }else{
            cmd(rx_buf2);
         }
      }
      else if(i == 5){// link hal pin
         if(is_hal_pin(source) && is_hal_pin(sink)){
            link_hal_pins(source, sink);
            printf("OK %s <= %s = %f\n", sink, source, get_hal_pin(sink));
         }
         else if(is_hal_pin(sink)){// set hal pin
            set_hal_pin(sink, strtof(source,NULL));
            printf("OK %s = %f\n", sink, get_hal_pin(sink));
         }
         else{// hal pin not found
            printf("not found: %s\n", sink);
         }
      }
   }
);

ENDCOMP;