Commit 661f6d7c authored by schneider's avatar schneider
Browse files

feat(imutest): Show compass on screen

parent 7f16030c
...@@ -78,6 +78,7 @@ SRCS += GUI_Paint.c ...@@ -78,6 +78,7 @@ SRCS += GUI_Paint.c
SRCS += DEV_Config.c SRCS += DEV_Config.c
SRCS += font24.c SRCS += font24.c
SRCS += font24CN.c SRCS += font24CN.c
SRCS += font12.c
SRCS += card10.c SRCS += card10.c
SRCS += display.c SRCS += display.c
SRCS += leds.c SRCS += leds.c
......
...@@ -13,6 +13,7 @@ ...@@ -13,6 +13,7 @@
#include "gpio.h" #include "gpio.h"
#include "bhy_uc_driver.h" #include "bhy_uc_driver.h"
#include "pmic.h" #include "pmic.h"
#include "GUI_DEV/GUI_Paint.h"
#include "card10.h" #include "card10.h"
...@@ -20,12 +21,14 @@ ...@@ -20,12 +21,14 @@
#include <stdio.h> #include <stdio.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <stdbool.h>
#define M_PI 3.1415
/***** Definitions *****/ /***** Definitions *****/
/* should be greater or equal to 69 bytes, page size (50) + maximum packet size(18) + 1 */ /* should be greater or equal to 69 bytes, page size (50) + maximum packet size(18) + 1 */
#define FIFO_SIZE 300 #define FIFO_SIZE 300
#define ROTATION_VECTOR_SAMPLE_RATE 100 #define ROTATION_VECTOR_SAMPLE_RATE 10
#define MAX_PACKET_LENGTH 18 #define MAX_PACKET_LENGTH 18
#define OUT_BUFFER_SIZE 60 #define OUT_BUFFER_SIZE 60
...@@ -34,8 +37,70 @@ ...@@ -34,8 +37,70 @@
char out_buffer[OUT_BUFFER_SIZE] = " W: 0.999 X: 0.999 Y: 0.999 Z: 0.999 \r"; char out_buffer[OUT_BUFFER_SIZE] = " W: 0.999 X: 0.999 Y: 0.999 Z: 0.999 \r";
uint8_t fifo[FIFO_SIZE]; uint8_t fifo[FIFO_SIZE];
void draw_arrow(int angle, int color)
{
float sin = sinf(angle * 2 * M_PI / 360.);
float cos = cosf(angle * 2 * M_PI / 360.);
int x1 = 160/2 + 30;
int y1 = 80/2;
int x2 = x1 - sin * 30;
int y2 = y1 - cos * 30;
Paint_DrawLine(x1, y1, x2, y2, color, LINE_STYLE_SOLID, DOT_PIXEL_2X2);
sin = sinf((angle - 140) * 2 * M_PI / 360.);
cos = cosf((angle - 140) * 2 * M_PI / 360.);
int x3 = x2 - sin * 10;
int y3 = y2 - cos * 10;
Paint_DrawLine(x2, y2, x3, y3, color, LINE_STYLE_SOLID, DOT_PIXEL_2X2);
sin = sinf((angle + 140) * 2 * M_PI / 360.);
cos = cosf((angle + 140) * 2 * M_PI / 360.);
int x4 = x2 - sin * 10;
int y4 = y2 - cos * 10;
Paint_DrawLine(x2, y2, x4, y4, color, LINE_STYLE_SOLID, DOT_PIXEL_2X2);
}
/***** Functions *****/ /***** Functions *****/
static void sensors_callback_orientation(bhy_data_generic_t * sensor_data, bhy_virtual_sensor_t sensor_id)
{
static int prev = -1;
printf("azimuth=%05d, pitch=%05d, roll=%05d status=%d\n",
sensor_data->data_vector.x * 360 / 32768,
sensor_data->data_vector.y * 360 / 32768,
sensor_data->data_vector.z * 360 / 32768,
sensor_data->data_vector.status
);
int angle = sensor_data->data_vector.x * 360 / 32768;
if(angle != prev) {
Paint_Clear(BLACK);
int colors[] = {RED, YELLOW, YELLOW, GREEN};
int color = colors[sensor_data->data_vector.status];
draw_arrow(sensor_data->data_vector.x * 360 / 32768, color);
char buf[128];
//sprintf(buf, "Azimuth: %3d", angle);
//Paint_DrawString_EN(0, 0, buf, &Font12, BLACK, color);
sprintf(buf, "%3d", angle);
Paint_DrawString_EN(0, 30, buf, &Font24, BLACK, color);
Paint_DrawCircle(57,35, 4, color, DRAW_FILL_EMPTY, DOT_PIXEL_1X1);
LCD_Update();
prev = angle;
}
}
static void sensors_callback_vector(bhy_data_generic_t * sensor_data, bhy_virtual_sensor_t sensor_id) static void sensors_callback_vector(bhy_data_generic_t * sensor_data, bhy_virtual_sensor_t sensor_id)
{ {
printf("x=%05d, y=%05d, z=%05d status=%d\n", printf("x=%05d, y=%05d, z=%05d status=%d\n",
...@@ -134,32 +199,9 @@ int main(void) ...@@ -134,32 +199,9 @@ int main(void)
bhy_data_type_t packet_type; bhy_data_type_t packet_type;
BHY_RETURN_FUNCTION_TYPE result; BHY_RETURN_FUNCTION_TYPE result;
const gpio_cfg_t interrupt_pin = {PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP};
GPIO_Config(&interrupt_pin);
card10_init(); card10_init();
/* wait for the bhy trigger the interrupt pin go down and up again */
while (GPIO_InGet(&interrupt_pin));
while (!GPIO_InGet(&interrupt_pin));
card10_diag(); card10_diag();
/* the remapping matrix for BHI and Magmetometer should be configured here to make sure rotation vector is */
/* calculated in a correct coordinates system. */
int8_t bhy_mapping_matrix_config[3*3] = {0,-1,0,1,0,0,0,0,1};
int8_t mag_mapping_matrix_config[3*3] = {-1,0,0,0,1,0,0,0,-1};
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_ACC, bhy_mapping_matrix_config);
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_MAG, mag_mapping_matrix_config);
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_GYRO, bhy_mapping_matrix_config);
/* the sic matrix should be calculated for customer platform by logging uncalibrated magnetometer data. */
/* the sic matrix here is only an example array (identity matrix). Customer should generate their own matrix. */
/* This affects magnetometer fusion performance. */
float sic_array[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
bhy_set_sic_matrix(sic_array);
#if 0 #if 0
/* install the callback function for parse fifo data */ /* install the callback function for parse fifo data */
if(bhy_install_sensor_callback(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP, sensors_callback_rotation_vector)) if(bhy_install_sensor_callback(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP, sensors_callback_rotation_vector))
...@@ -171,7 +213,8 @@ int main(void) ...@@ -171,7 +213,8 @@ int main(void)
//if(bhy_install_sensor_callback(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, sensors_callback_vector)) //if(bhy_install_sensor_callback(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, sensors_callback_vector))
//if(bhy_install_sensor_callback(VS_TYPE_GRAVITY, VS_WAKEUP, sensors_callback_vector)) //if(bhy_install_sensor_callback(VS_TYPE_GRAVITY, VS_WAKEUP, sensors_callback_vector))
//if(bhy_install_sensor_callback(VS_TYPE_ACCELEROMETER, VS_WAKEUP, sensors_callback_vector)) //if(bhy_install_sensor_callback(VS_TYPE_ACCELEROMETER, VS_WAKEUP, sensors_callback_vector))
if(bhy_install_sensor_callback(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, sensors_callback_vector_uncalib)) //if(bhy_install_sensor_callback(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, sensors_callback_vector_uncalib))
if(bhy_install_sensor_callback(VS_TYPE_ORIENTATION, VS_WAKEUP, sensors_callback_orientation))
{ {
printf("Fail to install sensor callback\n"); printf("Fail to install sensor callback\n");
} }
...@@ -184,11 +227,13 @@ int main(void) ...@@ -184,11 +227,13 @@ int main(void)
} }
#endif #endif
/* enables the virtual sensor */ /* enables the virtual sensor */
//if(bhy_enable_virtual_sensor(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0)) //if(bhy_enable_virtual_sensor(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
//if(bhy_enable_virtual_sensor(VS_TYPE_GRAVITY, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0)) //if(bhy_enable_virtual_sensor(VS_TYPE_GRAVITY, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
//if(bhy_enable_virtual_sensor(VS_TYPE_ACCELEROMETER, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0)) //if(bhy_enable_virtual_sensor(VS_TYPE_ACCELEROMETER, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
if(bhy_enable_virtual_sensor(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0)) //if(bhy_enable_virtual_sensor(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
if(bhy_enable_virtual_sensor(VS_TYPE_ORIENTATION, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
{ {
printf("Fail to enable sensor id=%d\n", VS_TYPE_GEOMAGNETIC_FIELD); printf("Fail to enable sensor id=%d\n", VS_TYPE_GEOMAGNETIC_FIELD);
} }
...@@ -198,7 +243,7 @@ int main(void) ...@@ -198,7 +243,7 @@ int main(void)
{ {
/* wait until the interrupt fires */ /* wait until the interrupt fires */
/* unless we already know there are bytes remaining in the fifo */ /* unless we already know there are bytes remaining in the fifo */
while (!GPIO_InGet(&interrupt_pin) && !bytes_remaining); while (!GPIO_InGet(&bhi_interrupt_pin) && !bytes_remaining);
bhy_read_fifo(fifo + bytes_left_in_fifo, FIFO_SIZE - bytes_left_in_fifo, &bytes_read, &bytes_remaining); bhy_read_fifo(fifo + bytes_left_in_fifo, FIFO_SIZE - bytes_left_in_fifo, &bytes_read, &bytes_remaining);
bytes_read += bytes_left_in_fifo; bytes_read += bytes_left_in_fifo;
......
...@@ -24,6 +24,8 @@ ...@@ -24,6 +24,8 @@
#define SPI_SPEED (15 * 1000 * 1000 * 1) // Bit Rate. Display has 15 MHz limit #define SPI_SPEED (15 * 1000 * 1000 * 1) // Bit Rate. Display has 15 MHz limit
const gpio_cfg_t bhi_interrupt_pin = {PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP};
void card10_init(void) void card10_init(void)
{ {
printf("card10 init...\n"); printf("card10 init...\n");
...@@ -63,13 +65,33 @@ void card10_init(void) ...@@ -63,13 +65,33 @@ void card10_init(void)
} }
if(bhy_driver_init(bhy1_fw)) {
printf("Failed to init bhy\n");
}
display_init(); display_init();
leds_init(); leds_init();
GPIO_Config(&bhi_interrupt_pin);
if(bhy_driver_init(bhy1_fw)) {
printf("Failed to init bhy\n");
} else {
/* wait for the bhy trigger the interrupt pin go down and up again */
while (GPIO_InGet(&bhi_interrupt_pin));
while (!GPIO_InGet(&bhi_interrupt_pin));
/* the remapping matrix for BHI and Magmetometer should be configured here to make sure rotation vector is */
/* calculated in a correct coordinates system. */
int8_t bhy_mapping_matrix_config[3*3] = {0,-1,0,1,0,0,0,0,1};
int8_t mag_mapping_matrix_config[3*3] = {-1,0,0,0,1,0,0,0,-1};
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_ACC, bhy_mapping_matrix_config);
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_MAG, mag_mapping_matrix_config);
bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_GYRO, bhy_mapping_matrix_config);
/* the sic matrix should be calculated for customer platform by logging uncalibrated magnetometer data. */
/* the sic matrix here is only an example array (identity matrix). Customer should generate their own matrix. */
/* This affects magnetometer fusion performance. */
float sic_array[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
bhy_set_sic_matrix(sic_array);
}
} }
static uint32_t ecg_read_reg(uint8_t reg) static uint32_t ecg_read_reg(uint8_t reg)
......
...@@ -4,6 +4,8 @@ ...@@ -4,6 +4,8 @@
#include <stdint.h> #include <stdint.h>
extern const gpio_cfg_t bhi_interrupt_pin;
void card10_init(void); void card10_init(void);
void card10_diag(void); void card10_diag(void);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment