Example #1
0
inline static
void main_event_loop(void)
{
  /** Frame parser FSM state */
  typedef enum {
    STF_MAGIC,
    STF_COMMAND,
    STF_LENGTH,
    STF_PARAM,
    STF_CHECKSUM0,
    STF_CHECKSUM1,
  } frame_state_t;
  frame_state_t fstate = STF_MAGIC;

  /** Frame parser offset/index into magic/data */
  uint8_t idx = 0;
  /** Frame parser cached data for current frame */
  uint8_t cmd = 0;
  /** Frame parser cached data for current frame */
  uint8_t len = 0;

  /** The UART checksum initialization code in uart-comm.c runs
   *  "uart_recv_checksum_reset()", i.e. we do not need to run it here
   *  a second time for the initial state.
   */

  /* Firmware FSM State */
  firmware_state_t pstate = STP_READY;

  /* Globally enable interrupts */
  sei();

  /* Firmware main event loop */
  while (1) {

    /* check for "measurement finished" event */
    if (GF_ISSET(GF_MEASUREMENT_FINISHED)) {
      pstate = firmware_handle_measurement_finished(pstate);
      GF_CLEAR(GF_MEASUREMENT_FINISHED);
      continue;
    }

    /* check whether a key event occured */
    if (switch_trigger_measurement()) {
      pstate = firmware_handle_switch_pressed(pstate);
      continue;
    }

    /* check whether a byte has arrived via UART */
    if (bit_is_set(UCSR0A, RXC0)) {
      /* The loop condition from uart_getc() checking UCSR0A for RCX0
       * has just been checked here, so we directly read UDR0 instead
       * of calling uart_getc(). */
      const uint8_t byte = (uint8_t) UDR0;
      uart_recv_checksum_update(byte);

      frame_state_t next_fstate = fstate;

      switch (fstate) {
      case STF_MAGIC:
        if (byte == pgm_read_byte_near(&(magic_header[idx++]))) {
          if (idx < 4) {
            next_fstate = STF_MAGIC;
          } else {
            next_fstate = STF_COMMAND;
          }
        } else {
          /* syncing, not an error */
          goto restart;
        }
        break;
      case STF_COMMAND:
        cmd = byte;
        next_fstate = STF_LENGTH;
        break;
      case STF_LENGTH:
        len = byte;
        if (pstate == STP_READY) {
          /* We can only use the personality_param_sram buffer in the
           * STP_READY state. By not writing to the buffer after
           * transitioning from STP_READY, we keep the content of the
           * buffer from the "start measurement" command for sending
           * back later.
           */
          pparam_sram.length = len;
        }
        if (len == 0) {
          next_fstate = STF_CHECKSUM0;
        } else if ((len >= personality_param_size) &&
                   (len < MAX_PARAM_LENGTH)) {
          idx = 0;
          next_fstate = STF_PARAM;
        } else {
          /* whoever sent us that wrongly sized data frame made an error */
          /** \todo Find a way to report errors without resorting to
           *        sending free text. */
          send_text_P(PSTR("param length mismatch"));
          goto error_restart_nomsg;
        }
        break;
      case STF_PARAM:
        if (pstate == STP_READY) {
          /* We can only use the personality_param_sram buffer in the
           * STP_READY state. By not writing to the buffer after
           * transitioning from STP_READY, we keep the content of the
           * buffer from the "start measurement" command for sending
           * back later.
           */
          pparam_sram.params[idx] = byte;
        }
        idx++;
        if (idx < len) {
          next_fstate = STF_PARAM;
        } else {
          next_fstate = STF_CHECKSUM0;
        }
        break;
      case STF_CHECKSUM0:
        /* We do not need to explicitly store the checksum here. A
         * checksum match will happen implicitly when the checksum
         * accumulator turns 0 which the code in uart-comm.c will
         * detect for us.
         */
        next_fstate = STF_CHECKSUM1;
        break;
      case STF_CHECKSUM1:
        /* We do not need to explicitly store the checksum here. A
         * checksum match will happen implicitly when the checksum
         * accumulator turns 0 which the code in uart-comm.c will
         * detect for us.
         */
        if (uart_recv_checksum_matches()) {
          /* checksum successful */
          pstate = firmware_handle_command(pstate, cmd);
          goto restart;
        } else {
          /** \todo Find a way to report checksum failure without
           *        resorting to sending free text. */
          send_text_P(PSTR("checksum fail"));
          goto error_restart_nomsg;
        }
        break;
      }
      //uprintf("idx=%u", idx);
      goto skip_errors;

    error_restart_nomsg:

    restart:
      next_fstate = STF_MAGIC;
      idx = 0;
      uart_recv_checksum_reset();

    skip_errors:
      fstate = next_fstate;

      continue;
    } /* character received on UART */

  } /* while (1) main event loop */

} /* void main_event_loop(void); */
Example #2
0
int main(void)
{
	initIO();

	while (1)
    {
        if (TCNT1<2000)
        {
            
        }
        
        if ((TCNT1>=2000) && (TCNT1<=5500) && (dataOK == 1))
        {
            //reset channels from high to low
            if (TCNT1>=CH1_out && bit_is_set(PORTB, PIN0)) PORTB &= ~(1<<PIN0);
            if (TCNT1>=CH2_out && bit_is_set(PORTB, PIN1)) PORTB &= ~(1<<PIN1);
            if (TCNT1>=CH3_out && bit_is_set(PORTB, PIN2)) PORTB &= ~(1<<PIN2);
            if (TCNT1>=CH4_out && bit_is_set(PORTB, PIN3)) PORTB &= ~(1<<PIN3);
            if (TCNT1>=CH5_out && bit_is_set(PORTB, PIN4)) PORTB &= ~(1<<PIN4);
            if (TCNT1>=CH6_out && bit_is_set(PORTD, PIN0)) PORTD &= ~(1<<PIN0);
            if (TCNT1>=CH7_out && bit_is_set(PORTD, PIN1)) PORTD &= ~(1<<PIN1);
            if (TCNT1>=CH8_out && bit_is_set(PORTD, PIN2)) PORTD &= ~(1<<PIN2);
            if (TCNT1>=CH9_out && bit_is_set(PORTD, PIN3)) PORTD &= ~(1<<PIN3);
            if (TCNT1>=CH10_out && bit_is_set(PORTD, PIN4)) PORTD &= ~(1<<PIN4);
            if (TCNT1>=CH11_out && bit_is_set(PORTD, PIN5)) PORTD &= ~(1<<PIN5);
            if (TCNT1>=CH12_out && bit_is_set(PORTD, PIN6)) PORTD &= ~(1<<PIN6);
        }
        
        if ((TCNT1>5500) && (TCNT1<35000))
        {
            CH1_out = uniq(rxbuffer[1], rxbuffer[2]);
            CH2_out = uniq(rxbuffer[3], rxbuffer[4]);
            CH3_out = uniq(rxbuffer[5], rxbuffer[6]);
            CH4_out = uniq(rxbuffer[7], rxbuffer[8]);
            CH5_out = uniq(rxbuffer[9], rxbuffer[10]);
            CH6_out = uniq(rxbuffer[11], rxbuffer[12]);
            CH7_out = uniq(rxbuffer[13], rxbuffer[14]);
            CH8_out = uniq(rxbuffer[15], rxbuffer[16]);
            CH9_out = uniq(rxbuffer[17], rxbuffer[18]);
            CH10_out = uniq(rxbuffer[19], rxbuffer[20]);
            CH11_out = uniq(rxbuffer[21], rxbuffer[22]);
            CH12_out = uniq(rxbuffer[23], rxbuffer[24]);
            
            //ready for output
            dataOK = 1;
        }
        
        
	}
	return 0; // never reached
}
Example #3
0
void adns9500_checks(void)
{
  uint8_t it;
  uint8_t byte;

  // For each ADNS
  for(it=1;it<=ADNS9500_NUM;it++)
  {
    // Set current ADNS CS active
    adns9500_spi_cs(it);
    _delay_us(ADNS9500_TIMINGS_NCS_SCLK);

    //-------------------------------
    // Read Motion register

    // Read register current value
    adns9500_spi_send(ADNS9500_SPIREGISTER_MOTION);
    _delay_us(ADNS9500_TIMINGS_SRAD);
    byte = adns9500_spi_recv();

    DEBUG(ADNS9500_ERROR,"ADNS9500 #%u motion=0x%X",it,byte);

    // Check if ADNS is Fault, if true, error.
    if( bit_is_set(byte,ADNS9500_MOTION_BIT_FAULT) )
    {
      adns9500_spi_cs(0);
      ERROR(ADNS9500_ERROR,
              "ADNS9500 #%u : ADNS is fault, motion=0x%X",
              it,byte);
    }

    // Check LP_CFG* consistency, if not, error.
    if( !bit_is_set(byte,ADNS9500_MOTION_BIT_LPVALID) )
    {
      adns9500_spi_cs(0);
      ERROR(ADNS9500_ERROR,
              "ADNS9500 #%u : LP_CFG* inconsistent, motion=0x%X",
              it,byte);
    }

    //-------------------------------
    // Read Observation register

    // delay read-write
    _delay_us(ADNS9500_TIMINGS_SRWSRR);

    // Clear observation register
    adns9500_spi_send(ADNS9500_SPI_WRITE|ADNS9500_SPIREGISTER_OBSERVATION);
    adns9500_spi_send(0x00);

    // wait for new frame
    //(DS don't exactly specify delay here, one frame seems to be max refresh time)
    _delay_us(ADNS9500_TIMINGS_FRAME_PERIOD);
    _delay_us(ADNS9500_TIMINGS_FRAME_PERIOD);

    // Read register current value
    adns9500_spi_send(ADNS9500_SPIREGISTER_OBSERVATION);
    _delay_us(ADNS9500_TIMINGS_SRAD);
    byte = adns9500_spi_recv();

    DEBUG(ADNS9500_ERROR,"ADNS9500 #%u observation=0x%X",it,byte);

    // Check if ADNS is running on SROM code, if not, error.
    if( !bit_is_set(byte,ADNS9500_OBSERVATION_BIT_OB7) )
    {
      adns9500_spi_cs(0);
      ERROR(ADNS9500_ERROR,
              "ADNS9500 #%u : ADNS is not running on SROM code, observation=0x%X",
              it,byte);
    }

    // Check if NPD pulse was detected, if true, error.
    if( bit_is_set(byte,ADNS9500_OBSERVATION_BIT_OB5) )
    {
      adns9500_spi_cs(0);
      ERROR(ADNS9500_ERROR,
              "ADNS9500 #%u : NPD pulse detected, observation=0x%X",
              it,byte);
    }

    //------------------------------------------------
    // Good to GO !
    adns9500_spi_cs(0);

  } /* for(it=1;it<=ADNS9500_NUM;it++) */
}
Example #4
0
File: UART.c Project: rubda/KMM
char uart_read_char()
{
	while(!bit_is_set(UCSR0A, RXC0));
	
	return UDR0;
}
Example #5
0
/*
 * Request SD to send us the slot:barcodes, then wiffle
 *  through them all labeling them.
 */
static void label_from_barcodes(UAContext *ua, int drive, bool label_encrypt)
{
   STORERES *store = ua->jcr->res.wstore;
   POOL_DBR pr;
   MEDIA_DBR mr, omr;
   vol_list_t *vl;
   dlist *vol_list = NULL;
   bool media_record_exists;
   char *slot_list;
   int max_slots;

   max_slots = get_num_slots_from_SD(ua);
   if (max_slots <= 0) {
      ua->warning_msg(_("No slots in changer to scan.\n"));
      return;
   }

   slot_list = (char *)malloc(nbytes_for_bits(max_slots));
   clear_all_bits(max_slots, slot_list);
   if (!get_user_slot_list(ua, slot_list, "slots", max_slots)) {
      goto bail_out;
   }

   vol_list = get_vol_list_from_SD(ua, store, false /* no listall */ , false /*no scan*/);
   if (!vol_list) {
      ua->warning_msg(_("No Volumes found to label, or no barcodes.\n"));
      goto bail_out;
   }

   /*
    * Display list of Volumes and ask if he really wants to proceed
    */
   ua->send_msg(_("The following Volumes will be labeled:\n"
                  "Slot  Volume\n"
                  "==============\n"));
   foreach_dlist(vl, vol_list) {
      if (!vl->VolName || !bit_is_set(vl->Slot - 1, slot_list)) {
         continue;
      }
      ua->send_msg("%4d  %s\n", vl->Slot, vl->VolName);
   }
   if (!get_yesno(ua, _("Do you want to label these Volumes? (yes|no): ")) ||
       (ua->pint32_val == 0)) {
      goto bail_out;
   }
   /*
    * Select a pool
    */
   memset(&pr, 0, sizeof(pr));
   if (!select_pool_dbr(ua, &pr)) {
      goto bail_out;
   }

   /*
    * Fire off the label requests
    */
   foreach_dlist(vl, vol_list) {
      if (!vl->VolName || !bit_is_set(vl->Slot - 1, slot_list)) {
         continue;
      }
      mr.clear();
      bstrncpy(mr.VolumeName, vl->VolName, sizeof(mr.VolumeName));
      media_record_exists = false;
      if (db_get_media_record(ua->jcr, ua->db, &mr)) {
         if (mr.VolBytes != 0) {
            ua->warning_msg(_("Media record for Slot %d Volume \"%s\" already exists.\n"),
                            vl->Slot, mr.VolumeName);
            mr.Slot = vl->Slot;
            mr.InChanger = mr.Slot > 0;  /* if slot give assume in changer */
            set_storageid_in_mr(store, &mr);
            if (!db_update_media_record(ua->jcr, ua->db, &mr)) {
               ua->error_msg(_("Error setting InChanger: ERR=%s"), db_strerror(ua->db));
            }
            continue;
         }
         media_record_exists = true;
      }
      mr.InChanger = mr.Slot > 0;  /* if slot give assume in changer */
      set_storageid_in_mr(store, &mr);

      /*
       * Deal with creating cleaning tape here. Normal tapes created in send_label_request() below
       */
      if (is_cleaning_tape(ua, &mr, &pr)) {
         if (media_record_exists) {      /* we update it */
            mr.VolBytes = 1;             /* any bytes to indicate it exists */
            bstrncpy(mr.VolStatus, "Cleaning", sizeof(mr.VolStatus));
            mr.MediaType[0] = 0;
            set_storageid_in_mr(store, &mr);
            if (!db_update_media_record(ua->jcr, ua->db, &mr)) {
                ua->error_msg("%s", db_strerror(ua->db));
            }
         } else {                        /* create the media record */
            if (pr.MaxVols > 0 && pr.NumVols >= pr.MaxVols) {
               ua->error_msg(_("Maximum pool Volumes=%d reached.\n"), pr.MaxVols);
               goto bail_out;
            }
            set_pool_dbr_defaults_in_media_dbr(&mr, &pr);
            bstrncpy(mr.VolStatus, "Cleaning", sizeof(mr.VolStatus));
            mr.MediaType[0] = 0;
            set_storageid_in_mr(store, &mr);
            if (db_create_media_record(ua->jcr, ua->db, &mr)) {
               ua->send_msg(_("Catalog record for cleaning tape \"%s\" successfully created.\n"),
                  mr.VolumeName);
               pr.NumVols++;          /* this is a bit suspect */
               if (!db_update_pool_record(ua->jcr, ua->db, &pr)) {
                  ua->error_msg("%s", db_strerror(ua->db));
               }
            } else {
               ua->error_msg(_("Catalog error on cleaning tape: %s"), db_strerror(ua->db));
            }
         }
         continue;                    /* done, go handle next volume */
      }
      bstrncpy(mr.MediaType, store->media_type, sizeof(mr.MediaType));

      /*
       * See if we need to generate a new passphrase for hardware encryption.
       */
      if (label_encrypt) {
         if (!generate_new_encryption_key(ua, &mr)) {
            continue;
         }
      }

      mr.Slot = vl->Slot;
      send_label_request(ua, &mr, &omr, &pr, false, media_record_exists, drive);
   }

bail_out:
   free(slot_list);
   if (vol_list) {
      free_vol_list(vol_list);
   }
   close_sd_bsock(ua);

   return;
}
void blink2(void) {
    if (bit_is_set(PORTA,PA2)) cbi(PORTA,PA2); else sbi(PORTA,PA2);
}
void blink(void) {
    if (bit_is_set(PORTA,PA0)) cbi(PORTA,PA0); else sbi(PORTA,PA0);
}
Example #8
0
void stateCalcPositionNed_i(void)
{
  if (bit_is_set(state.pos_status, POS_NED_I)) {
    return;
  }

  int errno = 0;
  if (state.ned_initialized_i) {
    if (bit_is_set(state.pos_status, POS_NED_F)) {
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
      INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
      ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
      SetBit(state.pos_status, POS_ENU_I);
      INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
      ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
    } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
      /* transform ecef_f -> ned_f, set status bit, then convert to int */
      ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
      SetBit(state.pos_status, POS_NED_F);
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
      ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_ECEF_F);
      ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
      SetBit(state.pos_status, POS_NED_F);
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
    } else { /* could not get this representation,  set errno */
      errno = 1;
    }
  } else if (state.utm_initialized_f) {
    if (bit_is_set(state.pos_status, POS_NED_F)) {
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
      INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_ENU_F)) {
      ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
      SetBit(state.pos_status, POS_ENU_I);
      INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
      /* transform utm_f -> ned_f -> ned_i, set status bits */
      NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
      SetBit(state.pos_status, POS_NED_F);
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
      SetBit(state.pos_status, POS_NED_F);
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
      LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
      SetBit(state.pos_status, POS_LLA_F);
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
      SetBit(state.pos_status, POS_NED_F);
      NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
    } else { /* could not get this representation,  set errno */
      errno = 2;
    }
  } else { /* ned coordinate system not initialized,  set errno */
    errno = 3;
  }
  if (errno) {
    //struct NedCoor_i _ned_zero = {0};
    //return _ned_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_NED_I);
}
//
// main function
//
int main(void) {

    char c;
    uint16_t loop = 0;
    uint8_t bootloaderEnableFlag = FALSE;


    // relocate interrupt vector table to bottom of flash
    // in case the bootloader will not be started
    myIVSELREG = _BV(IVCE);
    myIVSELREG = 0;        

    // init USART
    myUBRRH = (F_CPU/(BAUDRATE*16L)-1) >> 8;             // calculate baudrate and set high byte
    myUBRRL = (uint8_t)(F_CPU/(BAUDRATE*16L)-1);         // and low byte
    myUCSRB = _BV(myTXEN) | _BV(myRXEN) | _BV(myRXCIE);  // enable transmitter and receiver and receiver interrupt
    myUCSRC = myURSEL | _BV(myUCSZ1) | _BV(myUCSZ0);     // 8 bit character size, 1 stop bit, no parity

    // the bootloader may be activated either if
    // the character 'i' (interactive mode) was received from USART
    // or the flash is (still) empty

    // poll USART receive complete flag 64k times to catch the 'i' reliably
    do {
	if(bit_is_set(myUCSRA, myRXC))
	    if(myUDR == 'i')
		bootloaderEnableFlag = TRUE;
    } while(--loop);

    // test if flash is empty (i.e. flash content is 0xff)
    if(pgm_read_byte_near(0x0000) == 0xFF) {
	bootloaderEnableFlag = TRUE;  // set enable flag
    }

    // check enable flag and start application if FALSE
    if(bootloaderEnableFlag == FALSE) {
        myUCSRB = 0;  // clear USART register to reset default
        startApplication();  // start application code
    }


    //
    // now the bootloader code begins
    //


    // welcome message and prompt
    uartPutChar('\r');
    uartPutChar('>');

    // loop until a valid character is received
    do {

	c = uartGetChar();  // read a character

	if(c == 'f') {  // 'f' selects flash programming
	    uartPutChar('f');           // echo the 'f'
	    programThisMemory = FLASH;  // set flag
	}

#ifdef EEPROM_CODE
	if(c == 'e') {  // 'e' selects eeprom programming
	    uartPutChar('e');            // echo the 'e'
	    programThisMemory = EEPROM;  // set flag
	}
#endif

	if(c == 'g') {  // 'g' starts the application
	    uartPutChar('g');    // echo the 'g'
	    startApplication();  // and jump to 0x0000
	}

    } while(!programThisMemory);  // exit loop when a valid key was pressed


    // move interrupt vector table to boot loader area
    myIVSELREG = _BV(IVCE);
    myIVSELREG = _BV(IVSEL);
 
    uartPutChar('\r');  // set cursor to next line

    receiveBufferFull = FALSE;  // reset full flag
    receiveBufferPointer = (char *)receiveBuffer;  // reset buffer pointer to start of buffer

    // enable interrupts
    sei();

    // endless loop
    while(1) {

	// if buffer is full, parse the buffer and write to flash
	if(receiveBufferFull) {

	    cli();  // disable interrupts

	    // if parsing produced an error, restart bootloader
	    if(!parseSrecBuffer(receiveBuffer)) {
		uartPutChar('\r');  // set cursor to next line
		startBootloader();  // restart the bootloader
	    }

	    // was an end-of-file s-record found?
	    if(srecEndOfFile) {
		uartPutChar('O');  // 'OK' indicates successful programming
		uartPutChar('K');
		loop_until_bit_is_set(myUCSRA, myUDRE);  // wait until character is transmitted
		startBootloader();       // restart the bootloader
	    }

	    receiveBufferFull = FALSE;  // reset full flag
	    receiveBufferPointer = (char *)receiveBuffer;  // reset buffer pointer to start of buffer

	    sei();  // enable interrupts
	}
    }
}
int main(void)
{
	DDRB=0xFF;
	DDRD=0xFF;
	PORTB=0xFF;
	PORTD=0xFF;
	PORTA=0x00;

	//DDRD=0xFF;
	//PORTD=0x00;
	

	ICR1=10000;
	TCCR1A=(1<<COM1A1|1<<COM1B1|0<<COM1A0|0<<COM1B0|0<<FOC1A|0<<FOC1B|0<<WGM11|0<<WGM10); //check datasheet (can be written in hex/bin)
	TCCR1B=(1<<WGM13|1<<CS11|0<<CS12|0<<CS10|0<<WGM12|0<<ICNC1|0<<ICES1);    //check datasheet (can be written in hex/bin)


	for(;;)
	{
		Drive_motor(1,1);
		if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(10000,10000);
		}
		if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(10000,7500);
		}
		if((bit_is_set(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(7500,10000);
		}
		if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_clear(PINA,4)))
		{
			ocr_drive(10000,5000);
		}
		if((bit_is_clear(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_clear(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(5000,10000);
		}
		if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_clear(PINA,3))&&(bit_is_clear(PINA,4)))
		{
			ocr_drive(10000,2500);
		}
		if((bit_is_clear(PINA,0))&&(bit_is_clear(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(2500,10000);
		}
		if((bit_is_set(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_clear(PINA,4)))
		{
			ocr_drive(10000,0);
		}
		if((bit_is_clear(PINA,0))&&(bit_is_set(PINA,1))&&(bit_is_set(PINA,2))&&(bit_is_set(PINA,3))&&(bit_is_set(PINA,4)))
		{
			ocr_drive(0,10000);
		}
	}

}
Example #11
0
void stateCalcPositionEnu_f(void)
{
  if (bit_is_set(state.pos_status, POS_ENU_F)) {
    return;
  }

  int errno = 0;
  if (state.ned_initialized_f) {
    if (bit_is_set(state.pos_status, POS_NED_F)) {
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ENU_I)) {
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_NED_I)) {
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
      SetBit(state.pos_status, POS_NED_F);
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
      enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
    } else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
      /* transform ecef_i -> enu_i -> enu_f, set status bits */
      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
      ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
      SetBit(state.pos_status, POS_ECEF_I);
      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else { /* could not get this representation,  set errno */
      errno = 1;
    }
  } else if (state.utm_initialized_f) {
    if (bit_is_set(state.pos_status, POS_ENU_I)) {
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    } else if (bit_is_set(state.pos_status, POS_NED_F)) {
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_NED_I)) {
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
      SetBit(state.pos_status, POS_NED_F);
      VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
    } else if (bit_is_set(state.pos_status, POS_UTM_F)) {
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
      /* transform lla_f -> utm_f -> enu, set status bits */
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
      /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
      LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
      SetBit(state.pos_status, POS_LLA_F);
      utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
      SetBit(state.pos_status, POS_UTM_F);
      ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
    } else { /* could not get this representation,  set errno */
      errno = 2;
    }
  } else { /* ned coordinate system not initialized,  set errno */
    errno = 3;
  }
  if (errno) {
    //struct EnuCoor_f _enu_zero = {0.0f};
    //return _enu_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_ENU_F);
}
Example #12
0
/*!
 * \brief EEPROM emulator.
 *
 * Forces the chip to re-read the EEPROM contents and emulates a serial
 * EEPROM.
 *
 * If the hardware does not support this feature, then this call will
 * never return. Thus, make sure to have the driver properly configured.
 */
static void EmulateNicEeprom(void)
{
#ifdef __AVR_ENHANCED__
    register uint8_t clk;
    register uint8_t cnt;
    register uint8_t val;

    /*
     * Prepare the EEPROM emulation port bits. Configure the EEDO and
     * the EEMU lines as outputs and set EEDO to low and EEMU to high.
     */
    outb(PORTC, 0xC0);
    outb(DDRC, 0xC0);

    /*
     * Start EEPROM configuration. Stop/abort any activity and select
     * configuration page 3. Setting bit EEM0 will force the controller
     * to read the EEPROM contents.
     */

    /* Select page 3, stop and abort/complete. */
    nic_outlb(NIC_CR, NIC_CR_STP | NIC_CR_RD2 | NIC_CR_PS0 | NIC_CR_PS1);
    nic_outlb(NIC_PG3_EECR, NIC_EECR_EEM0);

    /*
     * We can avoid wasting port pins for EEPROM emulation by using the
     * upper bits of the address bus.
     */
    /*
     * No external memory access beyond this point.
     */
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
    cbi(XMCRA, SRE);
#else
    cbi(MCUCR, SRE);
#endif

    /*
     * Loop for all EEPROM words.
     */
    for(cnt = 0; cnt < sizeof(nic_eeprom); ) {

        /*
         *
         * 1 start bit, always high
         * 2 op-code bits
         * 7 address bits
         * 1 dir change bit, always low
         */
        for(clk = 0; clk < 11; clk++) {
            while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
            while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
        }

        /*
         * Shift out the high byte, MSB first. Our data changes at the EESK
         * rising edge. Data is sampled by the Realtek at the falling edge.
         */
        val = PRG_RDB(nic_eeprom + cnt);
        cnt++;
        for(clk = 0x80; clk; clk >>= 1) {
            while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
            if(val & clk)
                sbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
            while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
            cbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
        }

        /*
         * Shift out the low byte.
         */
        val = PRG_RDB(nic_eeprom + cnt);
        cnt++;
        for(clk = 0x80; clk; clk >>= 1) {
            while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
            if(val & clk)
                sbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
            while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
            cbi(RTL_EEDO_PORT, RTL_EEDO_BIT);
        }


        /* 5 remaining clock cycles. */
        for(clk = 0; clk < 5; clk++) {
            while(bit_is_clear(RTL_EESK_PIN, RTL_EESK_BIT));
            while(bit_is_set(RTL_EESK_PIN, RTL_EESK_BIT));
        }
    }

    /*
     * Enable memory interface.
     */
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
    sbi(XMCRA, SRE);
#else
    sbi(MCUCR, SRE);
#endif

    /* Reset port outputs to default. */
    outb(PORTC, 0x00);
    outb(DDRC, 0x00);
#endif
}
Example #13
0
static int DetectNicEeprom(void)
{
#ifdef __AVR_ENHANCED__
    register unsigned int cnt = 0;

    cli();
    /*
     * Prepare the EEPROM emulation port bits. Configure the EEDO
     * and the EEMU lines as outputs and set both lines to high.
     */
    outb(PORTC, 0xC0);
    outb(DDRC, 0xC0);

    /*
     * Force the chip to re-read the EEPROM contents.
     */
    nic_outlb(NIC_CR, NIC_CR_STP | NIC_CR_RD2 | NIC_CR_PS0 | NIC_CR_PS1);
    nic_outlb(NIC_PG3_EECR, NIC_EECR_EEM0);

    /*
     * No external memory access beyond this point.
     */
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
    cbi(XMCRA, SRE);
#else
    cbi(MCUCR, SRE);
#endif

    /*
     * Check, if the chip toggles our EESK input. If not, we do not
     * have EEPROM emulation hardware.
     */
    if(bit_is_set(PINC, 5)) {
        while(++cnt && bit_is_set(PINC, 5));
    }
    else {
        while(++cnt && bit_is_clear(PINC, 5));
    }

    /*
     * Enable memory interface.
     */
#if defined(__AVR_AT90CAN128__) || defined(__AVR_ATmega2561__)
    sbi(XMCRA, SRE);
#else
    sbi(MCUCR, SRE);
#endif

    /* Reset port outputs to default. */
    outb(PORTC, 0x00);
    outb(DDRC, 0x00);

    /* Wait until controller ready. */
    while(nic_inlb(NIC_CR) != (NIC_CR_STP | NIC_CR_RD2));

    sei();
    return cnt ? 0 : -1;
#else
    return -1;
#endif
}
Example #14
0
void maca_isr(void) {

//	print_packets("maca_isr");

	maca_entry++;

	if (bit_is_set(*MACA_STATUS, maca_status_ovr))
	{ PRINTF("maca overrun\n\r"); }
	if (bit_is_set(*MACA_STATUS, maca_status_busy))
	{ PRINTF("maca busy\n\r"); } 
	if (bit_is_set(*MACA_STATUS, maca_status_crc))
	{ PRINTF("maca crc error\n\r"); }
	if (bit_is_set(*MACA_STATUS, maca_status_to))
	{ PRINTF("maca timeout\n\r"); }

	if (data_indication_irq()) {
		*MACA_CLRIRQ = (1 << maca_irq_di);
		dma_rx->length = *MACA_GETRXLVL - 2; /* packet length does not include FCS */
		dma_rx->lqi = get_lqi();
		dma_rx->rx_time = *MACA_TIMESTAMP;

		/* check if received packet needs an ack */
		if(dma_rx->data[1] & 0x20) {
			/* this wait is necessary to auto-ack */
			volatile uint32_t wait_clk;
			wait_clk = *MACA_CLK + 200;
			while(*MACA_CLK < wait_clk) { continue; }
		}

		if(maca_rx_callback != 0) { maca_rx_callback(dma_rx); }

		add_to_rx(dma_rx);
		dma_rx = 0;
	}
	if (filter_failed_irq()) {
		PRINTF("maca filter failed\n\r");
		ResumeMACASync();
		*MACA_CLRIRQ = (1 << maca_irq_flt);
	}
	if (checksum_failed_irq()) {
		PRINTF("maca checksum failed\n\r");
		ResumeMACASync();
		*MACA_CLRIRQ = (1 << maca_irq_crc);
	}
	if (softclock_irq()) {
		*MACA_CLRIRQ = (1 << maca_irq_sftclk);
	}
	if (poll_irq()) {		
		*MACA_CLRIRQ = (1 << maca_irq_poll);
	}
	if(action_complete_irq()) {
		/* PRINTF("maca action complete %d\n\r", get_field(*MACA_CONTROL,SEQUENCE)); */
		if(last_post == TX_POST) {
			tx_head->status = get_field(*MACA_STATUS,CODE);
			if(maca_tx_callback != 0) { maca_tx_callback(tx_head); }
			dma_tx = 0;
			free_tx_head();
			last_post = NO_POST;
		}
		ResumeMACASync();
		*MACA_CLRIRQ = (1 << maca_irq_acpl);		
	}

	decode_status();

	if (*MACA_IRQ != 0)
	{ PRINTF("*MACA_IRQ %x\n\r", (unsigned int)*MACA_IRQ); }

	if(tx_head != 0) {
		post_tx();
	} else {
		post_receive();
	} 
}
Example #15
0
int
main (void)
{
#ifdef BOOTLOADER_SUPPORT
  _IVREG = _BV (IVCE);    /* prepare ivec change */
  _IVREG = _BV (IVSEL);   /* change ivec to bootloader */
#endif

  /* Default DDR Config */
#if IO_HARD_PORTS >= 4 && DDR_MASK_A != 0
  DDRA = DDR_MASK_A;
#endif
#if DDR_MASK_B != 0
  DDRB = DDR_MASK_B;
#endif
#if DDR_MASK_C != 0
  DDRC = DDR_MASK_C;
#endif
#if DDR_MASK_D != 0
  DDRD = DDR_MASK_D;
#endif
#if IO_HARD_PORTS >= 6
#if DDR_MASK_E != 0
  DDRE = DDR_MASK_E;
#endif
#if DDR_MASK_F != 0
  DDRF = DDR_MASK_F;
#endif
#endif
#if IO_HARD_PORTS >= 7
#if DDR_MASK_G != 0
  DDRG = DDR_MASK_G;
#endif
#endif


#ifdef STATUSLED_POWER_SUPPORT
  PIN_SET(STATUSLED_POWER);
#endif

  //FIXME: zum ethersex meta system hinzufügen, aber vor allem anderem initalisieren
  debug_init();
  debug_printf("%S (Debug mode)\n", pstr_E6_VERSION_STRING_LONG);

#ifdef DEBUG_RESET_REASON
  if (bit_is_set (mcusr_mirror, BORF))
    debug_printf("reset: Brown-out\n");
  else if (bit_is_set (mcusr_mirror, PORF))
    debug_printf("reset: Power on\n");
  else if (bit_is_set (mcusr_mirror, WDRF))
    debug_printf("reset: Watchdog\n");
  else if (bit_is_set (mcusr_mirror, EXTRF))
    debug_printf("reset: Extern\n");
  else
    debug_printf("reset: Unknown\n");
#endif

#ifdef BOOTLOADER_SUPPORT
  /* disable interrupts */
  cli ();
  wdt_disable();
#endif //BOOTLOADER_SUPPORT
  /* enable interrupts */
  sei ();

#ifdef USE_WATCHDOG
  debug_printf("enabling watchdog\n");
#ifdef DEBUG
  /* for debugging, test reset cause and jump to bootloader */
  if (MCU_STATUS_REGISTER & _BV (WDRF))
  {
    debug_printf("bootloader...\n");
    jump_to_bootloader();
  }
#endif
  /* set watchdog to 2 seconds */
  wdt_enable(WDTO_2S);
  wdt_kick();
#else //USE_WATCHDOG
  debug_printf("disabling watchdog\n");
  wdt_disable();
#endif //USE_WATCHDOG

#ifdef SPI_SUPPORT
  spi_init();
#endif

  ethersex_meta_init();

  /* must be called AFTER all other initialization */
#ifdef PORTIO_SUPPORT
  portio_init();
#elif defined(NAMED_PIN_SUPPORT)
  np_simple_init();
#endif

#ifdef ENC28J60_SUPPORT
  debug_printf ("enc28j60 revision 0x%x\n",
  read_control_register (REG_EREVID));
  debug_printf ("mac: %02x:%02x:%02x:%02x:%02x:%02x\n",
	  uip_ethaddr.addr[0], uip_ethaddr.addr[1], uip_ethaddr.addr[2],
	  uip_ethaddr.addr[3], uip_ethaddr.addr[4], uip_ethaddr.addr[5]);
#endif

#ifdef STATUSLED_BOOTED_SUPPORT
  PIN_SET(STATUSLED_BOOTED);
#endif

  ethersex_meta_startup();
  /* main loop */
  while (1)
  {
    wdt_kick();
    ethersex_meta_mainloop();

#ifdef SD_READER_SUPPORT
    if (sd_active_partition == NULL)
    {
      if (!sd_try_init())
      {
#ifdef VFS_SD_SUPPORT
        vfs_sd_try_open_rootnode();
#endif
      }
      wdt_kick();
    }
#endif

#ifdef BOOTLOADER_JUMP
    if (status.request_bootloader)
    {
#ifdef MBR_SUPPORT
      mbr_config.bootloader = 1;
      write_mbr();
#endif
#ifdef CLOCK_CRYSTAL_SUPPORT
      TIMER_8_AS_1_INT_OVERFLOW_OFF;
#endif
#ifdef DCF77_SUPPORT
      ACSR &= ~_BV (ACIE);
#endif
      cli();
      jump_to_bootloader();
    }
#endif

#ifndef TEENSY_SUPPORT
    if (status.request_wdreset)
    {
      cli();
      wdt_enable(WDTO_15MS);
      for (;;);
    }
#endif

    if (status.request_reset)
    {
      cli();
      void (*reset) (void) = NULL;
      reset();
    }
  }
}
Example #16
0
void manual_mode() 
{
    int discardSensors = 1;
    while(1) {
	    _delay_ms(1);
	    //printf("\r\nPlease enter the input in the format : <dir> <l1><l0> <r1><r0>\r\n");
	    //char dir   = getchar();
  	      if( bit_is_set(UCSR0A, RXC0) != 0) {
	       	    char dir = UDR0;
	 	    printf("UDR0 = %c\r\n",dir);

		    if(dir == 'a') {
			printf("Auto mode enabled\r\n");
			auto_mode();
		    }
		    while( (dir !='d') & (dir !='h') & (dir !='f') & (dir !='b') & (dir !='l') & (dir !='r') & (dir !='s') ) {
			printf("Enter valid direction character(f,b,l,r,s,d,h)");
			char dir   = getchar();
		    }
		    if(dir=='d') {
			discardSensors = 1;		// Discard Sensor Values
		    }
		    if(dir=='h') {
			discardSensors = 0; 	// Hold    Sensor Values
		    }

		    if(dir=='f') {
			int  speedl1 = getchar();
			int  speedl0 = getchar();
			int  speedr1 = getchar();
			int  speedr0 = getchar();
			printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
			int  speedl  = ((speedl1-48)*10  +  (speedl0-48))*255/99;
			int  speedr  = ((speedr1-48)*10  +  (speedr0-48))*255/99;

			moveForward(speedl,speedr);
			printf("Robot moves forward\r\n");
		    }
		    if(dir=='b') {
			int  speedl1 = getchar();
			int  speedl0 = getchar();
			int  speedr1 = getchar();
			int  speedr0 = getchar();
			printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
			int  speedl  = ((speedl1-48)*10  +  (speedl0-48))*255/99;
			int  speedr  = ((speedr1-48)*10  +  (speedr0-48))*255/99;

			moveBackward(speedl,speedr);
			printf("Robot moves backward\r\n");
		    }
		    if(dir=='r') {
			int  speedl1 = getchar();
			int  speedl0 = getchar();
			int  speedr1 = getchar();
			int  speedr0 = getchar();
			printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
			int  speedl  = ((speedl1-48)*10  +  (speedl0-48))*255/99;
			int  speedr  = ((speedr1-48)*10  +  (speedr0-48))*255/99;

			moveRight(speedl,speedr);
			printf("Robot moves right\r\n");
		    }
		    if(dir=='l') {
			int  speedl1 = getchar();
			int  speedl0 = getchar();
			int  speedr1 = getchar();
			int  speedr0 = getchar();
			printf("You entered input as : %c %c%c %c%c\r\n", dir, speedl1,speedl0,speedr1,speedr0);
			int  speedl  = ((speedl1-48)*10  +  (speedl0-48))*255/99;
			int  speedr  = ((speedr1-48)*10  +  (speedr0-48))*255/99;

			moveLeft(speedl,speedr);
			printf("Robot moves left\r\n");
		    }
		    if(dir=='s') {
			printf("You entered input as : %c\r\n", dir);

			moveStop();
			printf("Robot stops\r\n");
		    }

  	     }
	     else if( discardSensors!=1 ) {
			  _delay_ms(50);
			  int adc_val0=ReadADC(0);
			  _delay_ms(50);
			  int adc_val1=ReadADC(1);
			  _delay_ms(50);
			  int adc_val2=ReadADC(2);
			  printf("Sensor Readings :  ClifL = %d, ClifR = %d, Bump = %d\r\n",adc_val0,adc_val1,adc_val2);

			  if( (adc_val0 < 100) ) {
			  //if( (adc_val0 < 100) | (adc_val1 < 100) | (adc_val2 < 100) ) {
				moveStop();
				printf("Robot stops\r\n");
				_delay_ms(0500);
				
				moveBackward(160,160);
				printf("Robot moves backward\r\n");
				_delay_ms(1000);

				moveStop();
				printf("Robot stops\r\n");
				_delay_ms(0500);
				
				moveRight(160,160);
				printf("Robot moves Right\r\n");
				_delay_ms(0500);

				moveStop();
				printf("Robot stops\r\n");
				_delay_ms(0500);
			  }
            }
  }
}
Example #17
0
int main(void)               
{
 unsigned char i;
 unsigned char volatile licznikkl=0;  //zmienna wykorzystywana do pomiaru czasu
                             //naciśnięcia przycisków

 char volatile przyrost=1;  //przyrost zmiany współczynnika wypełnienia sygnału PWM
 //tablica komunikatów do wysłania
 char *info[5]={
                PSTR("\n\rRegulator obrotów silnika DC\n\r"),
                PSTR("0 - zatrzymanie silnika\n\r"),
                PSTR("1 - start z max. obrotami\n\r"),
                PSTR("N - podaj aktualne parametry sterownika\n\r\n"),
                PSTR("\n\rAktualne parametry PWM:")
               };

 union                       //unia pozwala na bajtowy dostęp do zmiennej int
      {
       unsigned int pwm;
       unsigned char pwmc[2];
      }volatile upwm;        //aktualny współczynnik wypełnienia sygn. PWM

 DDRB=0xff;    //PORTB - wy
 PORTB=0xff;   
 DDRD=0x02;    //PD1 - wy (RXD), pozostałe we
 PORTD=0x02;   //podciągania wejścia PD1 (RXD)
 UBRR=VUBRR;   //ustaw prędkość transmisji
 UCR=1<<RXCIE | 1<<TXCIE  | 1<<RXEN;   //zezwolenie na przerwania od
                   //odbiornika i nadajnika, zezwolenie na odbiór i nadawanie
 TCCR1A=0x83;      //PWM 10 bitowy
                   //zerowanie OC1 po spełnieniu warunku równości podczas liczenia
                   //w górę, ustawiane podczas liczenia w dół
 TCCR1B=0x01;      //preskaler=3, co przy 10-bit PWM daje Fwy=ok. 61Hz @8MHz
 TCNT1L=0x00;      //wstępne ustawienie licznika 1
 TCNT1H=0x00;
 upwm.pwm=0x3ff;   //początkowo silnik włączony, wartość TOP odpowiada wysokiemu
                   //poziomowi na wyjściu OC1 (PB3)
 OCR1H=upwm.pwmc[1];         //wpisz aktualnie ustawiony współczynnik do rejestrów
 OCR1L=upwm.pwmc[0];         //OCR1 timera1
 sei();            //włącz przerwania

 for(i=0;i<5;i++) //wyślij winietkę
 {
  wyslijtekstROM(info[i]);   //wysłanie pojedynczej linii tekstu
  while(bit_is_set(UCR,TXEN));    //trzeba zaczekać, aż zostanie wysłana do końca
 }

 while(1)          //główna pętla programu
 {
  if(fodbznak)          //czy odebrano jakiś znak?
  {
   fodbznak=0;          //tak
   switch (komenda)     //interpretacja komendy i wykonanie odpowiedniej akcji
   {
    case '.':                 //odebrano "." - zwiększ prędkość
              upwm.pwm+=przyrost;       //zwiększ PWM
              if(upwm.pwm>0x3ff)
              {
               upwm.pwm=0x3ff;    //jeśli przekroczono wartość TOP, to ustaw TOP
              }
              czekaj(150*tau);    //eliminacja powtórnej interpretacji
                                  //naciśnięcia przycisku
              licznikkl++;              //mierz długość naciśnięcia przycisku
              break;
    case ',':                 //odebrano "," - zmniejsz prędkość
              upwm.pwm-=przyrost;      //zmniejsz PWM
              if(upwm.pwm>0x3ff)
              {
               upwm.pwm=0;        //jeśli przekroczono wartość zero to ustaw zero
              }
              czekaj(150*tau);    //eliminacja powtórnej interpretacji
                                  //naciśnięcia przycisku
              licznikkl++;        //mierz długość naciśnięcia przycisku
              break;
    case '0':                //odebrano "0" - zatrzymaj silnik
              upwm.pwm=0;    //silnik STOP
              break;
    case '1':                //odebrano "1" - ustaw max obroty silnika
              upwm.pwm=0x3ff;     //silnik na MAX
              break;
    case 'n':
    case 'N':
              wyslijtekstROM(info[4]);      //wysłanie pojedynczej linii tekstu
              while(bit_is_set(UCR,TXEN));  //trzeba zaczekać,
                                            //aż zostanie wysłana do końca
              wyslijtekst("0x");       //wyślij prefiks dla liczb heksadecymalnych
              while(bit_is_set(UCR,TXEN));  //trzeba zaczekać,
                                            //aż zostanie wysłana do końca
              itoa(upwm.pwm,fifosio,16);   //konwersja liczby int (hex)
                                            //na łańcuch znakowy
              wyslijtekst(fifosio);        //wyślij aktualną wartość PWM do PC-ta
              while(bit_is_set(UCR,TXEN));  //trzeba zaczekać,
                                            //aż zostanie wysłana do końca
              break;
   }
   if(licznikkl>6)
   {
    przyrost=+16;             //wykryto długie naciśnięcie klawisza,
                              //zwiększ krok regulacji
    licznikkl=6;              //dalej już nie zwiększaj kroku
    cbi(PORTB,1);             //zapal diodę LED2
   }
   OCR1H=upwm.pwmc[1];        //wpisz aktualnie ustawiony współczynnik do rejestrów
   OCR1L=upwm.pwmc[0];        //OCR1 timera1
  }
  else
  {                           //jeśli cisza na linii, ustaw parametry spoczynkowe
   licznikkl=0;
   przyrost=1;
   sbi(PORTB,1);              //zgaś diodę LED2
  }
 }
} 
Example #18
0
// return TRUE if conversion is complete
u08 a2dIsComplete(void)
{
	return bit_is_set(ADCSR, ADSC);
}
Example #19
0
void blink1(void) {
    if (bit_is_set(PORTA,PA1)) cbi(PORTA,PA1); else sbi(PORTA,PA1);
}
Example #20
0
void maca_isr(void) {

//	print_packets("maca_isr");

    maca_entry++;

    if (bit_is_set(*MACA_STATUS, maca_status_ovr))
    {
        PRINTF("maca overrun\n\r");
    }
    if (bit_is_set(*MACA_STATUS, maca_status_busy))
    {
        PRINTF("maca busy\n\r");
    }
    if (bit_is_set(*MACA_STATUS, maca_status_crc))
    {
        PRINTF("maca crc error\n\r");
    }
    if (bit_is_set(*MACA_STATUS, maca_status_to))
    {
        PRINTF("maca timeout\n\r");
    }

    if (data_indication_irq()) {
        *MACA_CLRIRQ = (1 << maca_irq_di);
        dma_rx->length = *MACA_GETRXLVL - 2; /* packet length does not include FCS */
        dma_rx->lqi = get_lqi();
//		PRINTF("maca data ind %x %d\n\r", dma_rx, dma_rx->length);
        if(maca_rx_callback != 0) {
            maca_rx_callback(dma_rx);
        }
        add_to_rx(dma_rx);
        dma_rx = 0;
    }
    if (filter_failed_irq()) {
        PRINTF("maca filter failed\n\r");
        ResumeMACASync();
        *MACA_CLRIRQ = (1 << maca_irq_flt);
    }
    if (checksum_failed_irq()) {
        PRINTF("maca checksum failed\n\r");
        ResumeMACASync();
        *MACA_CLRIRQ = (1 << maca_irq_crc);
    }
    if (softclock_irq()) {
        *MACA_CLRIRQ = (1 << maca_irq_sftclk);
    }
    if (poll_irq()) {
        *MACA_CLRIRQ = (1 << maca_irq_poll);
    }
    if(action_complete_irq()) {
        /* PRINTF("maca action complete %d\n\r", get_field(*MACA_CONTROL,SEQUENCE)); */
        if(last_post == TX_POST) {
            if(maca_tx_callback != 0) {
                maca_tx_callback(tx_head);
            }
            dma_tx = 0;
            free_tx_head();
            last_post = NO_POST;
        }
        ResumeMACASync();
        *MACA_CLRIRQ = (1 << maca_irq_acpl);
    }

    decode_status();

    if (*MACA_IRQ != 0)
    {
        PRINTF("*MACA_IRQ %x\n\r", *MACA_IRQ);
    }

    if(tx_head != 0) {
        post_tx();
    } else {
        post_receive();
    }
}
Example #21
0
File: UART.c Project: rubda/KMM
void check_rx() {
	if (bit_is_set(UCSR0A, RXC0)){
		buffer_size = uart_read_string(buffer, 255);
	}
}
Example #22
0
/*
 * Save the new resource by chaining it into the head list for
 * the resource. If this is pass 2, we update any resource
 * pointers because they may not have been defined until
 * later in pass 1.
 */
bool save_resource(int type, RES_ITEM *items, int pass)
{
   URES *res;
   int rindex = type - R_FIRST;
   int i;
   int error = 0;

   /*
    * Ensure that all required items are present
    */
   for (i = 0; items[i].name; i++) {
      if (items[i].flags & CFG_ITEM_REQUIRED) {
         if (!bit_is_set(i, res_all.res_monitor.hdr.item_present)) {
               Emsg2(M_ERROR_TERM, 0, _("%s item is required in %s resource, but not found.\n"),
                  items[i].name, resources[rindex]);
         }
      }
      /* If this triggers, take a look at lib/parse_conf.h */
      if (i >= MAX_RES_ITEMS) {
         Emsg1(M_ERROR_TERM, 0, _("Too many items in %s resource\n"), resources[rindex]);
      }
   }

   /*
    * During pass 2 in each "store" routine, we looked up pointers
    * to all the resources referrenced in the current resource, now we
    * must copy their addresses from the static record to the allocated
    * record.
    */
   if (pass == 2) {
      switch (type) {
      /*
       * Resources not containing a resource
       */
      case R_MONITOR:
      case R_CLIENT:
      case R_STORAGE:
      case R_DIRECTOR:
      case R_CONSOLE_FONT:
         break;
      default:
         Emsg1(M_ERROR, 0, _("Unknown resource type %d in save_resource.\n"), type);
         error = 1;
         break;
      }
      /*
       * Note, the resource name was already saved during pass 1,
       * so here, we can just release it.
       */
      if (res_all.res_monitor.hdr.name) {
         free(res_all.res_monitor.hdr.name);
         res_all.res_monitor.hdr.name = NULL;
      }
      if (res_all.res_monitor.hdr.desc) {
         free(res_all.res_monitor.hdr.desc);
         res_all.res_monitor.hdr.desc = NULL;
      }
      return (error == 0);
   }

   /*
    * Common
    */
   if (!error) {
      res = (URES *)malloc(resources[rindex].size);
      memcpy(res, &res_all, resources[rindex].size);
      if (!res_head[rindex]) {
        res_head[rindex] = (RES *)res; /* store first entry */
         Dmsg3(900, "Inserting first %s res: %s index=%d\n",
               res_to_str(type), res->res_monitor.name(), rindex);
      } else {
         RES *next, *last;
         /*
          * Add new res to end of chain
          */
         for (last = next = res_head[rindex]; next; next=next->next) {
            last = next;
            if (strcmp(next->name, res->res_monitor.name()) == 0) {
               Emsg2(M_ERROR_TERM, 0,
                     _("Attempt to define second %s resource named \"%s\" is not permitted.\n"),
               resources[rindex].name, res->res_monitor.name());
            }
         }
         last->next = (RES *)res;
         Dmsg4(900, "Inserting %s res: %s index=%d pass=%d\n",
               res_to_str(type), res->res_monitor.name(), rindex, pass);
      }
   }
   return (error == 0);
}
Example #23
0
File: UART.c Project: rubda/KMM
void uart_send_char(uint8_t data)
{
	while(!bit_is_set(UCSR0A, UDRE0));
	UDR0 = data;
}
Example #24
0
/** \brief Function to be called when a message from FBW is available */
static inline void telecommand_task( void ) {
  uint8_t mode_changed = FALSE;
  copy_from_to_fbw();

  uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL);
  if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) {
    if  (too_far_from_home) {
      pprz_mode = PPRZ_MODE_HOME;
      mode_changed = TRUE;
    }
    if  (really_lost) {
      pprz_mode = RC_LOST_MODE;
      mode_changed = TRUE;
    }
  }
  if (bit_is_set(fbw_state->status, AVERAGED_CHANNELS_SENT)) {
    bool_t pprz_mode_changed = pprz_mode_update();
    mode_changed |= pprz_mode_changed;
#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
    bool_t calib_mode_changed = RcSettingsModeUpdate(fbw_state->channels);
    rc_settings(calib_mode_changed || pprz_mode_changed);
    mode_changed |= calib_mode_changed;
#endif
  }
  mode_changed |= mcu1_status_update();
  if ( mode_changed )
    PERIODIC_SEND_PPRZ_MODE(DefaultChannel);

#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
  /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
   * \a RADIO_ROLL and \a RADIO_PITCH \n
   */
  if (pprz_mode == PPRZ_MODE_AUTO1) {
    /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
    h_ctl_roll_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL);

    /** Pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */
    h_ctl_pitch_setpoint = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH);
  } /** Else asynchronously set by \a h_ctl_course_loop() */

  /** In AUTO1, throttle comes from RADIO_THROTTLE
      In MANUAL, the value is copied to get it in the telemetry */
  if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) {
    v_ctl_throttle_setpoint = fbw_state->channels[RADIO_THROTTLE];
  }
  /** else asynchronously set by v_ctl_climb_loop(); */

  mcu1_ppm_cpt = fbw_state->ppm_cpt;
#endif // RADIO_CONTROL


  vsupply = fbw_state->vsupply;
  current = fbw_state->current;

#ifdef RADIO_CONTROL
  if (!estimator_flight_time) {
    if (pprz_mode == PPRZ_MODE_AUTO2 && fbw_state->channels[RADIO_THROTTLE] > THROTTLE_THRESHOLD_TAKEOFF) {
      launch = TRUE;
    }
  }
#endif
}
Example #25
0
/* Save the new resource by chaining it into the head list for
 * the resource. If this is pass 2, we update any resource
 * pointers (currently only in the Job resource).
 */
void save_resource(int type, RES_ITEM *items, int pass)
{
    URES *res;
    int rindex = type - R_FIRST;
    int i;
    int error = 0;

    /*
     * Ensure that all required items are present
     */
    for (i = 0; items[i].name; i++) {
        if (items[i].flags & CFG_ITEM_REQUIRED) {
            if (!bit_is_set(i, res_all.res_dir.hdr.item_present)) {
                Emsg2(M_ABORT, 0, _("%s item is required in %s resource, but not found.\n"),
                      items[i].name, resources[rindex]);
            }
        }
    }

    /* During pass 2, we looked up pointers to all the resources
     * referrenced in the current resource, , now we
     * must copy their address from the static record to the allocated
     * record.
     */
    if (pass == 2) {
        switch (type) {
        /* Resources not containing a resource */
        case R_CONSOLE:
        case R_DIRECTOR:
            break;

        default:
            Emsg1(M_ERROR, 0, _("Unknown resource type %d\n"), type);
            error = 1;
            break;
        }
        /* Note, the resoure name was already saved during pass 1,
         * so here, we can just release it.
         */
        if (res_all.res_dir.hdr.name) {
            free(res_all.res_dir.hdr.name);
            res_all.res_dir.hdr.name = NULL;
        }
        if (res_all.res_dir.hdr.desc) {
            free(res_all.res_dir.hdr.desc);
            res_all.res_dir.hdr.desc = NULL;
        }
        return;
    }

    /* Common */
    if (!error) {
        res = (URES *)malloc(resources[rindex].size);
        memcpy(res, &res_all, resources[rindex].size);
        if (!res_head[rindex]) {
            res_head[rindex] = (RES *)res; /* store first entry */
        } else {
            RES *next, *last;
            for (last=next=res_head[rindex]; next; next=next->next) {
                last = next;
                if (bstrcmp(next->name, res->res_dir.hdr.name)) {
                    Emsg2(M_ERROR_TERM, 0,
                          _("Attempt to define second %s resource named \"%s\" is not permitted.\n"),
                          resources[rindex].name, res->res_dir.hdr.name);
                }
            }
            last->next = (RES *)res;
            Dmsg2(90, "Inserting %s res: %s\n", res_to_str(type),
                  res->res_dir.hdr.name);
        }
    }
}
Example #26
0
void parse_ins_msg(void)
{
  uint8_t offset = 0;
  if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);

    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x,
                                   &xsens_gps_arm_y, &xsens_gps_arm_z);
  } else if (xsens_id == XSENS_Error_ID) {
    xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
  } else if (xsens_id == XSENS_MTData2_ID) {
    for (offset = 0; offset < xsens_len;) {
      uint8_t code1 = xsens_msg_buf[offset];
      uint8_t code2 = xsens_msg_buf[offset + 1];
      int subpacklen = (int)xsens_msg_buf[offset + 2];
      offset += 3;


      if (code1 == 0x10) {
        if (code2 == 0x10) {
          // UTC
        } else if (code2 == 0x20) {
          // Packet Counter
        }
        if (code2 == 0x30) {
          // ITOW
        }
      } else if (code1 == 0x20) {
        if (code2 == 0x30) {
          // Attitude Euler
          ins_phi   = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
          ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
          ins_psi   = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;

          new_ins_attitude = 1;

        }
      } else if (code1 == 0x40) {
        if (code2 == 0x10) {
          // Delta-V
          ins_ax = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
          ins_ay = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
          ins_az = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
        }
      } else if (code1 == 0x80) {
        if (code2 == 0x20) {
          // Rate Of Turn
          ins_p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
          ins_q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
          ins_r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
        }
      } else if (code1 == 0x30) {
        if (code2 == 0x10) {
          // Baro Pressure
        }
      } else if (code1 == 0xE0) {
        if (code2 == 0x20) {
          // Status Word
          xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
          //gps.tow = xsens_msg_statusword;
#if USE_GPS_XSENS
          if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
            if (bit_is_set(xsens_msg_statusword, 23) && bit_is_set(xsens_msg_statusword, 24)) {
              gps.fix = GPS_FIX_3D;
            } else {
              gps.fix = GPS_FIX_2D;
            }
          } else { gps.fix = GPS_FIX_NONE; }
#endif
        }
      } else if (code1 == 0x88) {
        if (code2 == 0x40) {
          gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
          gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
          gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
          gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
          gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
        } else if (code2 == 0xA0) {
          // SVINFO
          gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);

#if USE_GPS_XSENS
          gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);

          gps.last_3dfix_ticks = sys_time.nb_sec_rem;
          gps.last_3dfix_time = sys_time.nb_sec;

          uint8_t i;
          // Do not write outside buffer
          for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
            uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
            if (ch > gps.nb_channels) { continue; }
            gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
            gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
            gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
            gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
          }
#endif
        }
      } else if (code1 == 0x50) {
        if (code2 == 0x10) {
          //gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
        } else if (code2 == 0x20) {
          // Altitude Elipsoid
          gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;

          // Compute geoid (MSL) height
          float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon);
          gps.hmsl =  gps.utm_pos.alt - (geoid_h * 1000.0f);
          SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);

          //gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt;
        } else if (code2 == 0x40) {
          // LatLong
#ifdef GPS_LED
          LED_TOGGLE(GPS_LED);
#endif
          gps.last_3dfix_ticks = sys_time.nb_sec_rem;
          gps.last_3dfix_time = sys_time.nb_sec;
          gps.week = 0; // FIXME
          lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
          lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));

          // Set the real UTM zone
          gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1;

          utm_f.zone = nav_utm_zone0;
          // convert to utm
          utm_of_lla_f(&utm_f, &lla_f);
          // copy results of utm conversion
          gps.utm_pos.east = utm_f.east * 100;
          gps.utm_pos.north = utm_f.north * 100;
          SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);

          gps_xsens_publish();
        }
      } else if (code1 == 0xD0) {
        if (code2 == 0x10) {
          // Velocity
          ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset));
          ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset));
          ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset));
          gps.ned_vel.x = ins_vx;
          gps.ned_vel.y = ins_vy;
          gps.ned_vel.z = ins_vx;
          SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
        }
      }

      if (subpacklen < 0) {
        subpacklen = 0;
      }
      offset += subpacklen;
    }


    /*

      gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
      gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
      gps.pdop = 5;  // FIXME Not output by XSens
    */

    /*
     */


    /*
      ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
      ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
      ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
    */


    /*
      xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
      #if USE_GPS_XSENS
      gps.tow = xsens_time_stamp;
      #endif
    */

  }

}
Example #27
0
void parse_ins_msg( void ) {
  uint8_t offset = 0;
  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
    xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
  }
  else if (xsens_id == XSENS_ReqOutputSettings_ID) {
    xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
  }
  else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
    xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );

    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
  }
  else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
    xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
    xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
    xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);

    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
  }
  else if (xsens_id == XSENS_Error_ID) {
    xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
  }
#ifdef USE_GPS_XSENS
  else if (xsens_id == XSENS_GPSStatus_ID) {
    gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
    gps.num_sv = 0;

    gps.last_fix_time = cpu_time_sec;

    uint8_t i;
    // Do not write outside buffer
    for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
      uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
      if (ch > gps.nb_channels) continue;
      gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
      gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
      gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
      gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
      if (gps.svinfos[ch].flags > 0)
      {
        gps.num_sv++;
      }
    }
  }
#endif
  else if (xsens_id == XSENS_MTData_ID) {
    /* test RAW modes else calibrated modes */
    //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
      if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
        ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
        ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
        ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
        offset += XSENS_DATA_RAWInertial_LENGTH;
      }
      if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
#ifdef GPS_LED
    LED_TOGGLE(GPS_LED);
#endif
        gps.last_fix_time = cpu_time_sec;
        gps.week = 0; // FIXME
        gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
        gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
        gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
        gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);


        /* Set the real UTM zone */
        gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;

        lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
        lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
        utm_f.zone = nav_utm_zone0;
        /* convert to utm */
        utm_of_lla_f(&utm_f, &lla_f);
        /* copy results of utm conversion */
        gps.utm_pos.east = utm_f.east*100;
        gps.utm_pos.north = utm_f.north*100;
        gps.utm_pos.alt = gps.lla_pos.alt;

        ins_x = utm_f.east;
        ins_y = utm_f.north;
        // Altitude: Xsens LLH gives ellipsoid height
        ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;

	// Compute geoid (MSL) height
        float hmsl;
	WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
        gps.hmsl =  XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);

        ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
        ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
        ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
        gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
        gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
        gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
        gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
        gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
        gps.pdop = 5;  // FIXME Not output by XSens
#endif
        offset += XSENS_DATA_RAWGPS_LENGTH;
      }
    //} else {
      if (XSENS_MASK_Temp(xsens_output_mode)) {
        offset += XSENS_DATA_Temp_LENGTH;
      }
      if (XSENS_MASK_Calibrated(xsens_output_mode)) {
        uint8_t l = 0;
        if (!XSENS_MASK_AccOut(xsens_output_settings)) {
          ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
          ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
          ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
          l++;
        }
        if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
          ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
          ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
          ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
          l++;
        }
        if (!XSENS_MASK_MagOut(xsens_output_settings)) {
          ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
          ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
          ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
          l++;
        }
        offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
      }
      if (XSENS_MASK_Orientation(xsens_output_mode)) {
        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
          float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
          float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
          float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
          float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
          float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
          float dcm01 =       2 * (q1*q2 + q0*q3);
          float dcm02 =       2 * (q1*q3 - q0*q2);
          float dcm12 =       2 * (q2*q3 + q0*q1);
          float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
          ins_phi   = atan2(dcm12, dcm22);
          ins_theta = -asin(dcm02);
          ins_psi   = atan2(dcm01, dcm00);
          offset += XSENS_DATA_Quaternion_LENGTH;
        }
        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
          ins_phi   = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
          ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
          ins_psi   = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
          offset += XSENS_DATA_Euler_LENGTH;
        }
        if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
          offset += XSENS_DATA_Matrix_LENGTH;
        }
        new_ins_attitude = 1;
      }
      if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
        uint8_t l = 0;
        if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
          l++;
        }
        if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
          l++;
        }
        offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
      }
      if (XSENS_MASK_Position(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
        gps.last_fix_time = cpu_time_sec;

        lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
        lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
        gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
        gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
        gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
        /* convert to utm */
        utm_of_lla_f(&utm_f, &lla_f);
        /* copy results of utm conversion */
        gps.utm_pos.east = utm_f.east*100;
        gps.utm_pos.north = utm_f.north*100;
        ins_x = utm_f.east;
        ins_y = utm_f.north;
        ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
        gps.hmsl = ins_z * 1000;
#endif
        offset += XSENS_DATA_Position_LENGTH;
      }
      if (XSENS_MASK_Velocity(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
        ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
        ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
        ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
#endif
        offset += XSENS_DATA_Velocity_LENGTH;
      }
      if (XSENS_MASK_Status(xsens_output_mode)) {
        xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
        if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
        else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
        else gps.fix = GPS_FIX_NONE;
#endif
        offset += XSENS_DATA_Status_LENGTH;
      }
      if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
        xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
        gps.tow = xsens_time_stamp;
#endif
        offset += XSENS_DATA_TimeStamp_LENGTH;
      }
      if (XSENS_MASK_UTC(xsens_output_settings)) {
        xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
        xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
        xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
        xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
        xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
        xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
        xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);

        offset += XSENS_DATA_UTC_LENGTH;
      }
    //}
  }

}
Example #28
0
File: pwm.c Project: slawx/pwmsign
int main(void) {
    uint8_t i, j;

    char c;
    int8_t bxi = -1;
    char buffer[ROWS];

    // OSCCAL = 101; // determined experimentally for /my/ ATtiny2313

    uart_38400();

    // UCSRB |= (1 << RXCIE); // enable rx interrupt. causes glitches.

    printstr_p(PSTR("\nWelcome to the LED sign.\nTo update, type the letter 'u' followed by 32 bytes and a newline.\n"));

    sei();

    // synch_init();

    ROW_PORT = 0xff; // 1 == off
    ROW_DDR = 0xff;
    COLUMN_DDR = _BV(5) | _BV(4) | _BV(3) | _BV(2);

    TCCR0B = _BV(CS01) | _BV(CS00); // ck / 64
    OCR0A = 0;
    TIMSK = _BV(TOIE0) | _BV(OCIE0A); // enable overflow and compare interrupts

    sei();

    while (1) {
        if (intflags.sort) {
            intflags.sort = 0;
            pwm_copy(p[pdi % 2], bitmap[pdi], ROWS);
        }
        while (bit_is_set(UCSRA, RXC) && !intflags.sort) {
            c = UDR;
            if (bxi < 0) {
                if (c == 'u') { // 'update'
                    printstr_p(PSTR("u"));
                    bxi++;
                    break;
                }
            } else if (bxi < (ROWS * COLUMNS)) {
                buffer[bxi % ROWS] = c;
                if((bxi % ROWS) == (ROWS-1)) {
                    j = (bxi / ROWS);
                    for(i=0; i<ROWS; i++) {
                        bitmap[j][i] = buffer[i];
                    }
                    itoa(j, buffer, 10);
                    printstr(buffer);
                }
                bxi++;
            } else {
                printstr_p(PSTR(" OK\n"));
                bxi = -1;
            }
        }
    }
    return 0;
}
Example #29
0
void adc_wait_stop()
{
	cbi(ADCSRA, ADIE);
	while(bit_is_set(ADCSRA, ADSC));
}
Example #30
0
File: CUL.c Project: jknofe/a-culfw
int
main(void)
{
  wdt_enable(WDTO_2S);
#if defined(CUL_ARDUINO)
  clock_prescale_set(clock_div_1); 		// for 8MHz clock div schould be 1
#endif

  MARK433_PORT |= _BV( MARK433_BIT ); // Pull 433MHz marker
  MARK915_PORT |= _BV( MARK915_BIT ); // Pull 915MHz marker

  // if we had been restarted by watchdog check the REQ BootLoader byte in the
  // EEPROM ...
  if(bit_is_set(MCUSR,WDRF) && erb(EE_REQBL)) {
    ewb( EE_REQBL, 0 ); // clear flag
    start_bootloader();
  }


  // Setup the timers. Are needed for watchdog-reset
  OCR0A  = 249;                            // Timer0: 0.008s = 8MHz/256/250
  TCCR0B = _BV(CS02);
  TCCR0A = _BV(WGM01);
  TIMSK0 = _BV(OCIE0A);

  TCCR1A = 0;
  TCCR1B = _BV(CS11) | _BV(WGM12);         // Timer1: 1us = 8MHz/8

  MCUSR &= ~(1 << WDRF);                   // Enable the watchdog

  led_init();
  spi_init();
  eeprom_init();
  USB_Init();
  fht_init();
  tx_init();
  input_handle_func = analyze_ttydata;
#ifdef HAS_RF_ROUTER
  rf_router_init();
  display_channel = (DISPLAY_USB|DISPLAY_RFROUTER);
#else
  display_channel = DISPLAY_USB;
#endif

  checkFrequency(); 
  LED_OFF();

  for(;;) {
    USB_USBTask();
    CDC_Task();
    RfAnalyze_Task();
    Minute_Task();
#ifdef HAS_FASTRF
    FastRF_Task();
#endif
#ifdef HAS_RF_ROUTER
    rf_router_task();
#endif
#ifdef HAS_ASKSIN
    rf_asksin_task();
#endif
#ifdef HAS_MORITZ
    rf_moritz_task();
#endif
#ifdef HAS_RWE
    rf_rwe_task();
#endif
#ifdef HAS_RFNATIVE
    native_task();
#endif
#ifdef HAS_KOPP_FC
    kopp_fc_task();
#endif
#ifdef HAS_MBUS
    rf_mbus_task();
#endif
#ifdef HAS_ZWAVE
    rf_zwave_task();
#endif

  }
}