예제 #1
0
void FreeIMUFormatFlash(void)
{
	char outbuff[20];

	memcpy(outbuff, "Formatting FLASH\n", 17);
	VCP_write(outbuff, 17);

	FlashChipErase();
	EEPROMInit();

	memcpy(outbuff, "Done\n", 5);
	VCP_write(outbuff, 5);
}
uint16_t USB_writeFloat(float value)
{
	float local = value;
	char str[5];
	sprintf((char *)str, "%2.2f\r\n", local);
	return VCP_write(str, strlen(str));
}
예제 #3
0
파일: main.c 프로젝트: kmos/janusb
int execComand(Comand received){
	memset(response,'\0',256);
	switch(received.name){
		case C_ON :
			if(received.ID == 3){
				BSP_LED_On(LED3);
				sprintf(response,"{ \"opstatus\" : \"ok\ , \"id\" : %d }\n",received.ID);
				VCP_write(&response,256);
			}
예제 #4
0
int main(void)
{
	HAL_Init();
	SystemClock_Config();
	USBD_Init(&USBD_Device, &VCP_Desc, 0);

	USBD_RegisterClass(&USBD_Device, &USBD_CDC);
	USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_$$PROJECTNAME$$_fops);
	USBD_Start(&USBD_Device);

	char byte;
	for (;;)
	{
		if (VCP_read(&byte, 1) != 1)
			continue;
		VCP_write("\r\nYou typed ", 12);
		VCP_write(&byte, 1);
		VCP_write("\r\n", 2);
	}
}
예제 #5
0
void FreeIMURaw(void)
{
	char outbuff[60];
	Point3df accel_xyz, gyro_xyz, mag_xyz;

	accelerometer_read(&accel_xyz);
	gyro_read(&gyro_xyz);
	MagPointRaw(&mag_xyz);

	sprintf(outbuff, "%d,%d,%d,%d,%d,%d,%d,%d,%d,0,0,\n",(int)accel_xyz.x, (int)accel_xyz.y, (int)accel_xyz.z, (int)gyro_xyz.x, (int)gyro_xyz.y, (int)gyro_xyz.z, (int)mag_xyz.x, (int)mag_xyz.y, (int)mag_xyz.z);
	VCP_write(outbuff, strlen(outbuff));
}
uint16_t USB_readLine(void * ptrBuffer)
{
	int i = 0;
	char receivedChar;
	do {
		// wait to receive char
		while(VCP_read(&receivedChar, 1)==0);

		if (receivedChar!='\r')
		{
			#ifdef READLINE_ECHO_CHARS
			// echo?
			VCP_write(&receivedChar, 1);
			#endif

			// add this char to ptrBuffer
			*((char*)ptrBuffer) = receivedChar;
			ptrBuffer++;
		}
		else
		{
			// the \r was sent, so append the terminator char \0
			#ifdef READLINE_ECHO_CHARS
			// echo?
			VCP_write("\r\n", 2);
			#endif

			// end string
			*((char*)ptrBuffer) = '\0';
		}

		i++;
	} while(receivedChar != '\r' && i < READLINE_MAX_LENGTH);

	return i-1;
}
예제 #7
0
파일: syscall.c 프로젝트: kmos/nodoCentrale
int _write(int file, char *ptr, int len) {
	VCP_write((uint8_t*)ptr, len);
	return len;
}
예제 #8
0
int main(void)
{

  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  MX_USB_DEVICE_Init();

  /* USER CODE BEGIN 2 */
  HAL_PCDEx_SetConnectionState(hUsbDeviceFS.pData, connectionState);// USB Activate
  HAL_Delay(3000);
  printf("Hello world.\r\n");

  uint8_t buf = 0;
  uint8_t cbuf[100];
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */

    // USART1 <=> VCP echo test
    if (USB_IS_ACTIVE())
    {
      HAL_Delay(5000);

      printf("\r\nVCP Connected > ");
      fflush(stdout);
      VCP_write("\r\nVCP Connected > ", 18);

      while (USB_IS_ACTIVE())
      {
        // VCP to USART1
        if (VCP_read(&buf, 1) == 1 && buf)
        {
          printf("\033[36m%c\033[0m", buf);
          fflush(stdout);
          VCP_write(&buf, 1);
          buf = 0;
        }

        // USART1 to VCP
        HAL_UART_Receive(&huart1, &buf, 1, 1);
        if (buf)
        {
          sprintf(cbuf, "\033[36m%c\033[0m", buf);
          VCP_write(&cbuf, 10);
          printf("%c", buf);
          fflush(stdout);
          buf = 0;
        }
      }

      printf("\r\nVCP Disconnected.\r\n");
    }
    else
    {
      // [debug] dump device status
      printf("\rdev_state = %d", hUsbDeviceFS.dev_state);
      fflush(stdout);
      HAL_Delay(500);
    }
  }
  /* USER CODE END 3 */

}
void USB_writeNewLine()
{
	VCP_write("\r\n", 2);
}
uint16_t USB_writeLine(void* ptrBuffer)
{
	int wchars = VCP_write(ptrBuffer, strlen((char*)ptrBuffer));
	USB_writeNewLine();
	return wchars;
}
예제 #11
0
void FreeIMUVersion(void)
{
	char buffer[20];
	sprintf(buffer, "AHRS ver:%d.%d\r\n", VERSION_MAJOR, VERSION_MINOR);
	VCP_write(buffer, strlen(buffer));
}
예제 #12
0
void FreeIMUReadCalibration(void)
{
	char outbuff[100];
	int pos=0;

	EEPROMGet(VAR_CALIBRATION, (uint8_t*)&calibration);

	// Accelerometer offsets
	memcpy(outbuff, "acc offset: ", 12);
	pos+=12;
	hex_to_ascii((unsigned char*)&calibration.offsets[0], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.offsets[1], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.offsets[2], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = '\n';

	VCP_write(outbuff, pos);

	// Magnetomer offsets
	pos=0;
	memcpy(outbuff, "mag offset: ", 12);
	pos+=12;
	hex_to_ascii((unsigned char*)&calibration.offsets[3], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.offsets[4], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.offsets[5], &outbuff[pos], sizeof(uint16_t));
	pos += sizeof(uint16_t)*2;
	outbuff[pos++] = '\n';

	VCP_write(outbuff, pos);

	// Accelerometer scale
	pos=0;
	memcpy(outbuff, "acc scale: ", 11);
	pos+=11;
	hex_to_ascii((unsigned char*)&calibration.scales[0], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.scales[1], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.scales[2], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = '\n';

	VCP_write(outbuff, pos);

	//Magnetometer scale
	pos=0;
	memcpy(outbuff, "mag scale: ", 11);
	pos+=11;
	hex_to_ascii((unsigned char*)&calibration.scales[3], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.scales[4], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = ',';
	hex_to_ascii((unsigned char*)&calibration.scales[5], &outbuff[pos], sizeof(float));
	pos += sizeof(float)*2;
	outbuff[pos++] = '\n';

	VCP_write(outbuff, pos);
}
예제 #13
0
void FreeIMUSendYawPitchRoll(int count)
{
	Point3df gyro_xyz;
	Point3df accel_xyz;
	Point3df mag_xyz;
	char outbuff[60];
	int pos=0, i;

	if(filter_init == 0){
		memset(&gyro_xyz_filtered, 0, sizeof(Point3df));
		filter_init = 1;
	}

	ExpLedOn(ORANGE_LED);

	for(i=0; i<count; i++){
		ExpLedToggle(ORANGE_LED);

		gyro_read(&gyro_xyz);
		accelerometer_read(&accel_xyz);
		MagPointRaw(&mag_xyz);

		// Apply calibration values
		accel_xyz.x = (accel_xyz.x - (float)calibration.offsets[0]) / calibration.scales[0];
		accel_xyz.y = (accel_xyz.y - (float)calibration.offsets[1]) / calibration.scales[1];
		accel_xyz.z = (accel_xyz.z - (float)calibration.offsets[2]) / calibration.scales[2];

		mag_xyz.x = (mag_xyz.x - (float)calibration.offsets[3]) / calibration.scales[3];
		mag_xyz.y = (mag_xyz.y - (float)calibration.offsets[4]) / calibration.scales[4];
		mag_xyz.z = (mag_xyz.z - (float)calibration.offsets[5]) / calibration.scales[5];

		// Normalize acceleration measurements so they range from 0 to 1
		float accxnorm = accel_xyz.x/sqrtf(accel_xyz.x*accel_xyz.x+accel_xyz.y*accel_xyz.y+accel_xyz.z*accel_xyz.z);
		float accynorm = accel_xyz.y/sqrtf(accel_xyz.x*accel_xyz.x+accel_xyz.y*accel_xyz.y+accel_xyz.z*accel_xyz.z);

		// calculate pitch and roll
		float Pitch = asinf(-accxnorm);
		float Roll = asinf(accynorm/cosf(Pitch));

		// tilt compensated magnetic sensor measurements
		float magxcomp = mag_xyz.x*cosf(Pitch)+mag_xyz.z*sinf(Pitch);
		float magycomp = mag_xyz.x*sinf(Roll)*sinf(Pitch)+mag_xyz.y*cosf(Roll)-mag_xyz.z*sinf(Roll)*cosf(Pitch);

		// arctangent of y/x converted to degrees
		float heading = (float)(180*atan2f(magycomp,magxcomp)/PI);
		if(heading < 0)
		heading +=360;
		previous_time = (float)HAL_GetTick();

/*
		float tau=0.075;
		float a=0.0;
		float Complementary(float newAngle, float newRate,int looptime) {
		float dtC = float(looptime)/1000.0;
		a=tau/(tau+dtC);
		x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
		return x_angleC;
		}
*/
/*
		// Adjust the gyro readings with a complimentary filter
		// angle = 0.98 *(angle+gyro*dt) + 0.02*acc
		if(previous_time > 0){
		float dt = ((float)HAL_GetTick() - previous_time)/1000000;
		gyro_xyz.x = 0.98 *(gyro_xyz.x+gyro_xyz.x*dt) + 0.02*accel_xyz.x;
		gyro_xyz.y = 0.98 *(gyro_xyz.y+gyro_xyz.y*dt) + 0.02*accel_xyz.y;
		gyro_xyz.z = 0.98 *(gyro_xyz.z+gyro_xyz.z*dt) + 0.02*accel_xyz.z;
		}
*/
		pos = 0;
		hex_to_ascii((unsigned char*)&heading, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		hex_to_ascii((unsigned char*)&Pitch, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		hex_to_ascii((unsigned char*)&Roll, &outbuff[pos], sizeof(float));
		pos += sizeof(float)*2;

		outbuff[pos++] = ',';
		outbuff[pos++] = '\r';
		outbuff[pos++] = '\n';

		VCP_write(outbuff, pos);
	}

	ExpLedOff(ORANGE_LED);
}
예제 #14
0
void FreeIMUBaseInt(char count)
{
	char outbuff[100];
	char i;
	Point3df accel_xyz, gyro_xyz, mag_xyz;
	int pos=0;
	int16_t ival;

	ExpLedOn(ORANGE_LED);

	for(i=0; i<count; i++){
		ExpLedToggle(ORANGE_LED);

		accelerometer_read(&accel_xyz);
		gyro_read(&gyro_xyz);
		MagPointRaw(&mag_xyz);

		pos = 0;
		ival = (int16_t)accel_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)accel_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)accel_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)gyro_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.x;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.y;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		ival = (int16_t)mag_xyz.z;
		memcpy(&outbuff[pos], &ival, sizeof(int16_t));
		pos += sizeof(int16_t);

		outbuff[pos++] = '\r';
		outbuff[pos++] = '\n';

		VCP_write(outbuff, pos);
	}

	ExpLedOff(ORANGE_LED);
}