board/h8300/h8heli/servodrv/servodrv.c

   1 /* procesor H8S/2638 ver 1.1  */
   2 #include <stdio.h>
   3 #include <types.h>
   4 #include <cpu_def.h>
   5 #include <h8s2638h.h>
   6 #include <system_def.h>
   7 #include <string.h>
   8 #include <boot_fn.h>
   9 #include <periph/sci_rs232.h>
  10 #include "mycan.h"
  11 #include "servoout.h"
  12 #include "servodrv.h"
  13
  14
  15     /* void exit(int status) */
  16     /* { */
  17     /*   while(1); */
  18     /* } */
  19
  20 int cnt = 0;
  21 int ints = 0;
  22 int i = 0;
  23 int timer_1s = 0;
  24 int count_AD = 0;
  25
  26 #define MAXTIME 32767                   //Maximum of time_after_start
  27 #define STARTUP_TIME 50*15              //Time for calibration
  28 int time_after_start = 0;               //Counting time after start, will stop at MAXTIME
  29
  30 int heli_mode = HELI_MODE_INIT;
  31
  32 int timer_led_can0 = LED_SHORT_BLINK_TIME;
  33 int timer_led_can1 = LED_SHORT_BLINK_TIME;
  34
  35 int allow_direct_wd_refresh = 0;        //Hack for WD refresh during printf
  36
  37
  38 #define SER_MODE_IDLE 0
  39 #define SER_MODE_COMMAND 1
  40
  41 int serial_mode = SER_MODE_IDLE;
  42 int serial_command = 0;
  43 int awaited_chars = 0;
  44 int received_chars = 0;
  45
  46 #define REC_LENGTH 16
  47 char receive_array[REC_LENGTH];
  48
  49 //Debuging flags
  50 int debug_voltages = 0;
  51 int debug_CAN = 0;
  52 int debug_servos = 0;
  53 int debug_main_prog = 0;
  54
  55
  56 int ad0,ad1,ad2,ad3;
  57
  58 int ad0_val = 0;
  59 int ad1_val = 0;
  60 int ad2_val = 0;
  61 int ad3_val = 0;
  62
  63 #define ad0_mul 37
  64 #define ad0_shiftr 9
  65 #define ad1_mul 40
  66 #define ad1_shiftr 8
  67
  68 /* If you want to spare memory and only have support for SCI channels
  69  * 0 and 1 uncomment the following block. */
  70 #if 1
  71 sci_info_t *sci_rs232_chan_array[] = {
  72     &sci_rs232_chan0,
  73     &sci_rs232_chan1
  74 };
  75 #endif
  76
  77 static void deb_led_out(char val)
  78 {
  79     if (val&1)
  80         DEB_LED_ON(0);
  81     else
  82         DEB_LED_OFF(0);
  83     if (val&2)
  84         DEB_LED_ON(1);
  85     else
  86         DEB_LED_OFF(1);
  87     if (val&4)
  88         DEB_LED_ON(2);
  89     else
  90         DEB_LED_OFF(2);
  91     if (val&8)
  92         DEB_LED_ON(3);
  93     else
  94         DEB_LED_OFF(3);
  95 }
  96
  97 void init()
  98 {/* set shaddow registers */
  99 /*    DIO_P1DDR_shadow=0;
 100     DIO_P3DDR_shadow=0;  */
 101
 102 /*    *DIO_PCDR=0x00;
 103     SHADOW_REG_SET(DIO_PJDDR,0xee); / * set PJ.1, PJ.2, PJ.3 LED output * /
 104
 105     *DIO_P3DR|=0xc5;
 106     SHADOW_REG_SET(DIO_P3DDR,0x85); */
 107
 108     /* show something on debug leds */
 109     //deb_led_out(2);
 110     //*DIO_PJDR =0x00;
 111     //FlWait(1*100000);
 112
 113         //Initialize WD pin as output
 114     SHADOW_REG_SET(DIO_PCDDR,PCDDR_PC7DDRm);
 115
 116         DEB_LED_INIT();
 117
 118 }
 119
 120
 121 void led_blink(int* timer, int mask, int time) {
 122         if(time > *timer) {
 123                 DEB_LED_ON(mask);
 124                 *timer = time;
 125         }
 126 }
 127
 128 void debug_main(void) {
 129         printf("STATE: ");
 130         switch(heli_mode) {
 131                 case HELI_MODE_INIT:    printf("INIT, ");break;
 132                 case HELI_MODE_RUN:             printf("RUN,  ");break;
 133                 default:                                printf("????, ");break;
 134         }
 135         printf("Uptime: %i s, ",(time_after_start/50));
 136         printf("\n");
 137 }
 138
 139 //Interrupt routine
 140 void  LightOn(void) __attribute__ ((interrupt_handler));
 141 void LightOn(void)
 142 {
 143         *TPU_TSR1 &= ~TSR1_TCFVm ; //reset overflow flag
 144         *TPU_TCNT1 = 0xFFFF - 29*1000;
 145         //i = (i + 1) & 7;
 146         ints++;
 147         //FlWait(1*200000);
 148 }
 149
 150 int main() {
 151         int received;
 152         int out_num=-1,out_val=-1;
 153
 154
 155         cli(); //Stop interrupts
 156
 157         *DIO_PADR|=0x0f;
 158         *DIO_PADDR=0x0f;                /* A16-A19 are outputs */
 159
 160
 161         excptvec_initfill(LightOn,0);
 162
 163         excptvec_set(42,LightOn);
 164
 165         init_CAN_interrupts();
 166         init_servo_outputs();
 167
 168
 169     //init_hw();
 170         init();
 171
 172         i = 0;
 173
 174         deb_led_out(6);
 175         FlWait(1*2000);
 176
 177         sci_rs232_chan_default = 0;
 178         sci_rs232_setmode(57600, 0, 0, 0);
 179
 180         init_CAN0();
 181         init_CAN1();
 182
 183         init_AD_converter();
 184
 185         sti();
 186
 187         print_introduction();
 188         while (1) {
 189                 received = sci_rs232_recch(0);
 190                 if(received!=-1) {
 191                         switch (serial_mode) {
 192                         case SER_MODE_COMMAND:
 193                                 printf("Got char %c in command mode. Total chars: %d, awaiting: %d\n",received,received_chars,awaited_chars);
 194                                 receive_array[received_chars++] = (char)received;
 195                                 receive_array[received_chars] = 0;
 196                                 if(received_chars>=awaited_chars) {
 197
 198                                         switch(serial_command) {
 199                                                 case 'O':
 200                                                         printf("Got sentence: %s\n",receive_array);
 201                                                         sscanf(receive_array+1,"%4X",&out_val);
 202                                                         receive_array[1] = 0;
 203                                                         sscanf(receive_array,"%1X",&out_num);
 204                                                         printf("Decoded: %d, %X\n",out_num,out_val);
 205                                                         if((out_num < OUTPUTS_COUNT) && (out_num >= 0)) {
 206                                                                 servo_mirror_input[out_num] = out_val;
 207                                                         }
 208                                                         break;
 209                                                 case 'K':
 210                                                         printf("Got sentence: %s\n",receive_array);
 211                                                         sscanf(receive_array,"%2X",&out_val);
 212                                                         printf("Decoded: %X\n",out_val);
 213                                                         *rc_maska_can = out_val;
 214                                                         break;
 215
 216                                         }
 217                                         serial_mode = SER_MODE_IDLE;received_chars = 0;
 218                                 }
 219                                 if(received_chars>=REC_LENGTH-1) {
 220                                         printf("buffer full\n");
 221                                         serial_mode = SER_MODE_IDLE;received_chars = 0;
 222                                 }
 223                                 break;
 224
 225
 226                         case SER_MODE_IDLE:
 227                                 switch (received) {
 228                                         case 'H': case 'h': print_help();                                               break;
 229                                         case 'A': case 'a': rc_mode = RC_MODE_AUTO;                             break;
 230                                         case 'M': case 'm': rc_mode = RC_MODE_MANUAL;                   break;
 231                                         case 'C': case 'c': debug_CAN           = ~debug_CAN;           break;
 232                                         case 'V': case 'v': debug_voltages      = ~debug_voltages;      break;
 233                                         case 'S': case 's': debug_servos        = ~debug_servos;        break;
 234                                         case 'P': case 'p': debug_main_prog     = ~debug_main_prog;     break;
 235                                         case 'O': case 'o':     awaited_chars = 5; serial_mode = SER_MODE_COMMAND; serial_command = 'O'; break;
 236                                         case 'K': case 'k':     awaited_chars = 2; serial_mode = SER_MODE_COMMAND; serial_command = 'K'; break;
 237                                         case '?':                       print_short_status();                           break;
 238                                         default : printf("For help hit h.\n");                                  break;
 239                                 }
 240                                 received_chars = 0;
 241                                 break;
 242                         default: serial_mode = SER_MODE_IDLE; break;
 243                 }
 244         }
 245                 //printf(":%c:\n",received);
 246
 247
 248         //debug_out_CAN();
 249         //debug_pwm();
 250         if(servopulse!=0) {
 251                 servopulse = 0;
 252
 253                 timer_middle_interval();
 254                 watchdog_refresh();
 255         }
 256         FlWait(1*20000);
 257
 258
 259
 260   };
 261
 262 };
 263
 264 void print_introduction() {
 265         int temp_wd_rfr = allow_direct_wd_refresh;
 266
 267         allow_direct_wd_refresh = 1;
 268         printf("**************************************\n");
 269         printf("*  H E L I C O P T E R    S E R V O  *\n");
 270         printf("*         C O N T R O L L E R        *\n");
 271         printf("*------------------------------------*\n");
 272         printf("*      Ota Herm, DCE FEE CTU 2006    *\n");
 273         printf("**************************************\n");
 274         printf("Compiled: %s, %s\n",__DATE__,__TIME__);
 275         printf("For help hit h.\n\n");
 276
 277         allow_direct_wd_refresh = temp_wd_rfr;
 278 }
 279
 280 void print_help() {
 281         allow_direct_wd_refresh = 1;
 282
 283         print_introduction();
 284         printf("KEYS:\n");
 285         printf("H\tshows this help\n");
 286         printf("A\tsets to the AUTOMATIC mode\n");
 287         printf("M\tsets to the MANUAL mode\n");
 288         printf("V\ttoggles debugging of voltages\n");
 289         printf("S\ttoggles debugging of servo values\n");
 290         printf("C\ttoggles debugging of CAN\n");
 291         printf("P\ttoggles debugging of main program\n");
 292         printf("OxYYYY\tset value for output number x, value YYYY hexadecimal\n");
 293         printf("KYY\tset value for mask, value YY hexadecimal\n");
 294         printf("\n\n");
 295
 296         allow_direct_wd_refresh = 0;
 297 }
 298
 299 void print_short_status() {
 300         allow_direct_wd_refresh = 1;
 301         servo_debug_short();
 302         allow_direct_wd_refresh = 0;
 303 }
 304
 305 void set_led_living(int state) {
 306     if (state) {
 307                 DEB_LED_ON(LED_LIVING);
 308         } else {
 309                 DEB_LED_OFF(LED_LIVING);
 310         }
 311 }
 312
 313 /**
 314 *       50Hz timer ... to be short as possible
 315 **/
 316 void timer_middle_interval(void) {
 317         if(heli_mode == HELI_MODE_INIT) {
 318                 //During the startup time blink the LED rapidly
 319                 if((timer_1s % 5) > 2) {
 320                         set_led_living(0);
 321                 } else {
 322                         set_led_living(1);
 323                 }
 324         } else {
 325                 //After startup time blink the LED slowly
 326                 if (timer_1s>25) {
 327                         set_led_living(0);
 328                 } else {
 329                         set_led_living(1);
 330                 }
 331         }
 332
 333         if(timer_led_can0 > 0) {
 334                 DEB_LED_ON(LED_CAN0);
 335                 timer_led_can0--;
 336                 if(timer_led_can0==0) DEB_LED_OFF(LED_CAN0);
 337         }
 338
 339         if(timer_led_can1 > 0) {
 340                 DEB_LED_ON(LED_CAN1);
 341                 timer_led_can1--;
 342                 if(timer_led_can1==0) DEB_LED_OFF(LED_CAN1);
 343         }
 344
 345
 346         if(timer_1s > 50) {
 347                 timer_1s = 0;
 348
 349                 start_AD_conversion();
 350
 351                 allow_direct_wd_refresh = 1;
 352                 if(debug_main_prog)             debug_main();
 353                 //servo_debug_fast();
 354                 if(debug_servos)        servo_debug_visual();
 355                 if(debug_CAN)           debug_out_CAN();
 356                 if(debug_voltages)      debug_AD_convertion();
 357
 358                 cnt++;
 359                 send_temp_message_CAN0();
 360                 send_temp_message_CAN1();
 361
 362                 allow_direct_wd_refresh = 0;
 363         }
 364         timer_1s++;
 365         if(time_after_start < MAXTIME) time_after_start++;
 366         if(time_after_start < STARTUP_TIME) {
 367                 heli_mode = HELI_MODE_INIT;
 368         } else {
 369                 heli_mode = HELI_MODE_RUN;
 370         }
 371
 372 }
 373
 374 void init_AD_converter(void) {
 375         *SYS_MSTPCRA &= ~MSTPCRA_MSTPA1m; //Switch the AD module ON
 376
 377         excptvec_set(28,int_AD_convert);
 378
 379         *AD_ADCSR |=  ADCSR_SCANm;      //Set to SCAN MODE
 380         *AD_ADCSR &= ~ADCSR_ADFm;       //Clear INTERRUPT FLAG
 381         *AD_ADCSR |=  ADCSR_ADIEm;      //Set the INTERRUPT MASK
 382
 383         //Set the channels AN0 to AN4
 384         *AD_ADCSR &= ~ADCSR_CH3m;*AD_ADCSR &= ~ADCSR_CH2m;*AD_ADCSR |=  ADCSR_CH1m;     *AD_ADCSR |=  ADCSR_CH0m;
 385
 386         //Set the LOWEST CONVERSION SPEED
 387         *AD_ADCR &= ~ADCR_CKS0m;*AD_ADCR &= ~ADCR_CKS1m;
 388
 389         //Trigger the convertion by software
 390         *AD_ADCR &= ~ADCR_TRGS0m;*AD_ADCR &= ~ADCR_TRGS1m;
 391
 392         //Now the AD converter should be prepared
 393 }
 394
 395
 396 void start_AD_conversion(void) {
 397         *AD_ADCSR |= ADCSR_ADSTm;
 398 }
 399
 400 void debug_AD_convertion(void) {
 401         printf("AD:ints:%d,",count_AD);
 402         printf("VAL:%02X%02X,%02X%02X,%02X%02X,%02X%02X\n", *AD_ADDRAH,*AD_ADDRAL,*AD_ADDRBH,*AD_ADDRBL,*AD_ADDRCH,*AD_ADDRCL,*AD_ADDRDH,*AD_ADDRDL);
 403         printf("val:%4X,%4X,%4X,%4X\n",ad0,ad1,ad2,ad3);
 404         printf("val:%4d,%4d,%4d,%4d\n",ad0_val,ad1_val,ad2_val,ad3_val);
 405 }
 406
 407 /**
 408 *       AD convertion finished interrupt
 409 **/
 410 void int_AD_convert(void) {
 411         unsigned int ad0_temp,ad1_temp;
 412
 413
 414         *AD_ADCSR &= ~ADCSR_ADFm;       //Clear INTERRUPT FLAG
 415         count_AD++;
 416
 417         ad0 = (((*AD_ADDRAH) << 8) + *AD_ADDRAL);
 418         ad1 = (((*AD_ADDRBH) << 8) + *AD_ADDRBL);
 419         ad2 = (((*AD_ADDRCH) << 8) + *AD_ADDRCL);
 420         ad3 = (((*AD_ADDRDH) << 8) + *AD_ADDRDL);
 421
 422         ad0_temp = (ad0 >> 6) & 0x03FF;ad1_temp = (ad1 >> 6) & 0x03FF;  ad2_val = (ad2 >> 6) & 0x03FF;  ad3_val = (ad3 >> 6) & 0x03FF;
 423         ad0_val = (ad0_temp * ad0_mul) >> ad0_shiftr;
 424         ad1_val = (ad1_temp * ad1_mul) >> ad1_shiftr;
 425         *AD_ADCSR &= ~ADCSR_ADSTm;
 426
 427 }
 428
 429 void watchdog_refresh(void) {
 430                 //Complement Watchdog
 431                 *DIO_PCDR ^= PCDR_PC7DRm;
 432 }