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(fault) = 0.0;
HAL_PIN(con) = 0.0;
HAL_PIN(jogvel) = 10.0;
HAL_PIN(jogpos) = 0.0;
HAL_PIN(jogging) = 0.0;

HAL_PIN(tim2_rv) = 420.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(volatile float jog_timeout) = 0.0;
MEM(volatile float jog_en) = 0.0;

INIT(
   hal_conf_init();
   if(hal_conf_load()){//flash load error
      hal.hal_state = CONFIG_LOAD_ERROR;
   }
);

RT(
   if(jog_timeout < 0.5){
      jog_timeout += period;
      PIN(jogpos) += jog_en * PIN(jogvel) * period;
      PIN(jogpos) = mod(PIN(jogpos));
   }else{
      PIN(jogging) = 0.0;
   }
   //TIM2->ARR = (int)CLAMP(PIN(tim2_rv), 1, 1500);
   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 print_version_info(struct version_info* v){
      printf(
          "%s v%i.%i.%i %s\n",
          v->product_name,
          v->major,
          v->minor,
          v->patch,
          v->git_version
      );
      printf("Branch %s\n",v->git_branch);
      printf("Compiled %s %s ",v->build_date, v->build_time);
      printf("by %s on %s\n",v->build_user, v->build_host);
   }

   void about(){
      
      //hv firmware. defined by the linker script
      extern const char _binary_obj_hv_hv_bin_start;
      extern const char _binary_obj_hv_hv_bin_size;
      extern const char _binary_obj_hv_hv_bin_end;

      //version info of bootloader
      volatile const struct version_info* bt_version_info = (void*)0x08000188;
      //version info of embedded hv firmware
      volatile const struct version_info* hv_version_info = (void*)(&_binary_obj_hv_hv_bin_start + 0x10c);
      
      printf("######## software info ########\n");
      print_version_info(&version_info);
      printf("GCC        %s\n",__VERSION__);
      printf("newlib     %s\n",_NEWLIB_VERSION);
      printf("CMSIS      %i.%i\n",__CM4_CMSIS_VERSION_MAIN,__CM4_CMSIS_VERSION_SUB);
      printf("StdPeriph  %i.%i.%i\n",__STM32F4XX_STDPERIPH_VERSION_MAIN,__STM32F4XX_STDPERIPH_VERSION_SUB1,__STM32F4XX_STDPERIPH_VERSION_SUB2);
      printf("CPU ID     %lx %lx %lx\n",U_ID[0], U_ID[1], U_ID[2]);
      CRC_ResetDR();
      uint32_t crc = CRC_CalcBlockCRC((uint32_t *) 0x08010000, 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");
      print_version_info(bt_version_info);
      
      printf("######## HV info ########\n");
      printf("hv: 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);
      print_version_info(hv_version_info);
   }

   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;

      RCC_ClocksTypeDef  RCC_ClocksStatus;
      RCC_GetClocksFreq(&RCC_ClocksStatus);

      printf("HSE_VALUE        = %uHz\n", HSE_VALUE);
      printf("SYSCLK_Frequency = %luHz\n", RCC_ClocksStatus.SYSCLK_Frequency );
      printf("HCLK_Frequency   = %luHz\n", RCC_ClocksStatus.HCLK_Frequency   );
      printf("PCLK1_Frequency  = %luHz\n", RCC_ClocksStatus.PCLK1_Frequency  );
      printf("PCLK2_Frequency  = %luHz\n", RCC_ClocksStatus.PCLK2_Frequency  );
      extern void *g_pfnVectors;
      printf("vtor %lu\n",(uint32_t)&g_pfnVectors);
      //*****************************************************************
      printf("RCC->CSR = %x\n", (unsigned int)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 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: getconf\n");
      printf("save conf: save\n");
      printf("load conf: load\n");
   }

   void cmd(char * s){
      if(!strcmp(s, "bootloader")){//reset and jump to bootloader
         *((unsigned long *)0x2001C000) = 0xDEADBEEF;//set bootloader trigger
         NVIC_SystemReset();
      }
      else if(!strcmp(s, "start")){
         printf("starting hal ... ");
         hal_start();
         printf("done\n");
      }
      else if(!strcmp(s, "stop")){
         printf("stopping hal ... ");
         hal_stop();
         printf("done\n");
      }
      else if(!strcmp(s, "hal")){
         hal_term_print_info();
      }
      else if(!strcmp(s, "about")){
         about();
      }
      else if(!strcmp(s, "sysinfo")){
         sysinfo();
      }
      else if(!strcmp(s, "reset")){
         NVIC_SystemReset();
      }
      else if(!strcmp(s, "list")){
         hal_term_list();
      }
      else if(!strcmp(s, "help")){
         help();
      }
      else if(!strcmp(s, "save")){
         hal_conf_save();
      }
      else if(!strcmp(s, "load")){
         int ret = hal_conf_load();
         if(ret == -1){
            printf("flash error\n");
            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 if(ret == -5){
            printf("CRC error\n");
         }else{
            printf("done\n");
         }
      }
      else if(!strcmp(s, "getconf")){
         hal_term_getconf();
      }
      else if(!strcmp(s, "jogl")){
         jog_en = -1.0;
         jog_timeout = 0.0;
         PIN(jogging) = 1.0;
      }
      else if(!strcmp(s, "jogr")){
         jog_en = 1.0;
         jog_timeout = 0.0;
         PIN(jogging) = 1.0;
      }
      else if(!strcmp(s, "jogx")){
         jog_en = 0.0;
         PIN(jogging) = 0.0;
      }
      else{
         printf("not found: %s\n",s);
      }
   }

   void fault_sender(){
     if(EDGE(PIN(fault))){
       switch((int)PIN(fault)){
         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;
         
         case STATE_HV_ERROR:
            printf("ERROR: HV error. iramx fault, or no communication.\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;

      buf[8 + 1] = 0;

      if (USB_CDC_is_connected() && PIN(send_step) > 0) {
         PIN(con) = 1.0;
         USB_VCP_send_string(buf);
      }else{
         PIN(con) = 0.0;
      }
   }

   PIN(buf_use) = bc;

   if (USB_CDC_is_connected() && systime >= 1000) {
      char source[APP_TX_BUF_SIZE];
      char sink[APP_TX_BUF_SIZE];

      fault_sender();
      if(EDGE(hal.hal_state)){
         hal_term_print_state();
      }

      int i = -1;
      char rx_buf[APP_TX_BUF_SIZE];
      if (USB_VCP_get_string(rx_buf)) {
         i = sscanf_(rx_buf, "%N = %N", sink, source);
      }
      if(i == 2){ // read hal pin
         if(hal_is_pin(sink)){
            hal_term_print_pin(hal_find_pin(sink));
         }
         else if(hal_is_compname(sink)){
            for(int i = 0; i < hal.hal_pin_count; i++){//find all pins of this comp
              if(!strncmp(hal.hal_pins[i]->name, sink, strlen(sink))){
                 hal_term_print_pin(hal.hal_pins[i]);
              }
            }
         }else{
            cmd(rx_buf);
         }
      }
      else if(i == 5){// link hal pin
         if(hal_is_pin(source) && hal_is_pin(sink)){
            hal_link_pins(source, sink);
            printf("OK %s <= %s = %f\n", sink, source, hal_get_pin(sink));
         }
         else if(hal_is_pin(sink)){// set hal pin
            hal_set_pin(sink, strtof(source,NULL));
            printf("OK %s = %f\n", sink, hal_get_pin(sink));
         }
         else{// hal pin not found
            printf("not found: %s\n", sink);
         }
      }
   }
);

ENDCOMP;