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));
}
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);
}
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);
}
}
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;
}
int _write(int file, char *ptr, int len) {
VCP_write((uint8_t*)ptr, len);
return len;
}
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;
}
void FreeIMUVersion(void)
{
char buffer[20];
sprintf(buffer, "AHRS ver:%d.%d\r\n", VERSION_MAJOR, VERSION_MINOR);
VCP_write(buffer, strlen(buffer));
}
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);
}
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);
}
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);
}
