static int aConfigure(int aFile)
{
CHECK(ADXL345_Init(aFile, ADXL345_ID, true));
struct ADXL345_DataFormat confDataFormat = {
.range = ADXL345_RANGE_2G,
};
CHECK(ADXL345_ConfigureDataFormat(aFile, &confDataFormat));
struct ADXL345_Power confPowerControl = {
.measurement = true,
};
CHECK(ADXL345_ConfigurePower(aFile, &confPowerControl));
return 0;
}
static int cConfigure(int cFile)
{
CHECK(HMC5883L_Init(cFile, HMC5883L_ID, true));
struct HMC5883L conf = {
.gain = HMC5883L_GAIN_1090,
.measurementMode = HMC5883L_MEASUREMENTMODE_NORMAL,
.outputRate = HMC5883L_OUTPUTRATE_30,
.samples = HMC5883L_SAMPLES_2,
};
CHECK(HMC5883L_Configure(cFile, &conf));
CHECK(HMC5883L_SetContinuousMeasurement(cFile));
return 0;
}
int main(void)
{
CPU_INT08U os_err;
//禁止CPU中断
CPU_IntDis();
//UCOS 初始化
OSInit(); /* Initialize "uC/OS-II, The Real-Time Kernel". */
//硬件平台初始化
BSP_Init(); /* Initialize BSP functions. */
/* Configure FSMC Bank1 NOR/PSRAM */
I2C_Ini();
HMC5883L_Init();
HMC5883L_Start();
MPU6050_Init();
//建立主任务， 优先级最高 建立这个任务另外一个用途是为了以后使用统计任务
os_err = OSTaskCreate((void (*) (void *)) App_TaskStart, //指向任务代码的指针
(void *) 0, //任务开始执行时，传递给任务的参数的指针
(OS_STK *) &App_TaskStartStk[APP_TASK_START_STK_SIZE - 1], //分配给任务的堆栈的栈顶指针 从顶向下递减
(INT8U) APP_TASK_START_PRIO); //分配给任务的优先级
os_err =os_err;
//ucos的节拍计数器清0 节拍计数器是0-4294967295 对于节拍频率100hz时， 每隔497天就重新计数
OSTimeSet(0);
OSStart(); /* Start multitasking (i.e. give control to uC/OS-II). */
/* Start multitasking (i.e. give control to uC/OS-II). */
return (0);
}
//Inicia o Magnetômetro para obtenção orientação magnética.
void configurar_bussola()
{
HMC5883L_InitTypeDef configuracao_inicial;
//HMC5883L_getMagScale(I2C3);
configuracao_inicial.Samples = _8_samples;
configuracao_inicial.Output_DataRate = _75_0_HZ;
//configuracao_inicial.Meas_mode = Positive_bias;
//configuracao_inicial.Meas_mode = Negative_bias;
configuracao_inicial.Meas_mode = Default_Meas;
configuracao_inicial.Gain = Gain_0_92;
configuracao_inicial.Mode = Countinuous;
configuracao_inicial.HS_I2C = 0;
HMC5883L_Init(I2C3, &configuracao_inicial);
HMC5883L_configIntPin(RCC_AHB1Periph_GPIOC, GPIOC, GPIO_Pin_7);
while(HMC5883L_checkDataReadyIntPin() == Bit_SET);
//float *offset = HMC5883L_getMagOffset(I2C3); //Valores encontrados dinamicamente
float offset[] = {0.1233, -0.4267, -0.21};
setar_offset_mag(offset);
}
// Digital Compass
int main(void)
{
uint8_t Res;
int16_t Com_Data[3];
uint16_t x0, y0;
unsigned long angle = 0;
double radian;
/* Setup the microcontroller system. Initialize the Embedded Flash Interface,
initialize the PLL and update the SystemFrequency variable. */
xSysCtlClockSet(72000000, xSYSCTL_OSC_MAIN | xSYSCTL_XTAL_8MHZ);
xSysCtlDelay(10000);
delay_init(72);
Lcd_Init();
HMC5883L_Init();
HMC5883L_Cfg(MODE_SIG | GAIN_1090 | SAMPLE_8 | DATA_RATE_15);
Lcd_Clear(GRAY0);
//
Gui_Circle(64, 80, 50, BLUE);
Gui_DrawFont_GBK16(64,14,BLUE,GRAY0,"N");
Gui_DrawFont_GBK16(64,132,BLUE,GRAY0,"S");
Gui_DrawFont_GBK16(5,80,BLUE,GRAY0,"W");
Gui_DrawFont_GBK16(116,80,BLUE,GRAY0,"E");
while(1){
Res = HMC5883L_DataGet(&Com_Data[0], &Com_Data[1], &Com_Data[2]);
if(Res) break;
angle = (unsigned long) (atan2((double)Com_Data[0],(double)Com_Data[1])*(180/3.14159265)+180);
//angle = rand()%360;
radian = angle * 3.1415926 / 180;
if(angle <= 180){
x0 = 64 + (int16_t)(40 * sin(radian));
y0 = 80 - (int16_t)(40 * cos(radian));
} else {
x0 = 64 + (int16_t)(40 * sin(radian));
y0 = 80 - (int16_t)(40 * cos(radian));
}
Gui_DrawLine(64, 80, x0, y0, BLUE);
delay_ms(500);
Gui_DrawLine(64, 80, x0, y0, GRAY0);
}
return 0;
}
int main(void)
{
int16_t data[9];
int16_t result[3];
int i=0;
SystemInit();
delay_init(72);
GPIO_Configuration();
Initial_UART1(115200L);
I2C_GPIO_Config();
NVIC_Configuration();
delay_ms(10);
Init_MPU6050();
delay_ms(10);
HMC5883L_Init();
delay_ms(10);
IMU_init();
Initial_Timer3();
system_microsec=micros();
while(1)
{
//delay_ms(10);
//if(micros()-system_microsec>upload_time)
{
Read_MPU6050_ACC(&data[0]);
Read_MPU6050_GYRO(&data[3]);
HMC5883L_Read(&data[6]);
IMU_getYawPitchRoll(result,data);
UART1_Put_Char(0xff);
UART1_Put_Char(0xaa);
/*out_int16_t(&data[0]);
out_int16_t(&data[1]);
out_int16_t(&data[2]);
out_int16_t(&data[3]);
out_int16_t(&data[4]);
out_int16_t(&data[5]);
out_int16_t(&data[6]);
out_int16_t(&data[7]);
out_int16_t(&data[8]);
*/
out_int16_t(&result[0]);
out_int16_t(&result[1]);
out_int16_t(&result[2]);
//out_int16_t(&_hlt);
//system_microsec = micros();
}
}
}
int main(void) {
char str_buffer[16];
SystemCoreClockUpdate();
gpioInit();
interruptInit();
adcInit();
Chip_PMU_GetSleepFlags(LPC_PMU);
Chip_PMU_ClearSleepFlags(LPC_PMU, PMU_PCON_DPDFLAG);
Chip_SPI_Init(SPI_PORT);
SysTick_Config(Chip_Clock_GetSystemClockRate() / TICKRATE_HZ);
//StuckI2CHack();
delayms(10);
MoonLander_I2C_Init(SENSOR_I2C, MOONLANDER_I2C_100K);
delayms(100);
// Initialize sensors:
HTU21D_Init(&g_HTU21D, SENSOR_I2C);
delayms(10);
HMC5883L_Init(&g_HMC5883L, SENSOR_I2C);
delayms(10);
HMC5883L_SetRange(&g_HMC5883L, HMC5883L_RANGE_2_5);
eGFX_InitDriver();
C12832A_Init(&g_C12832A, SPI_PORT, LCD_A0_PIN, LCD_RST_PIN, LCD_SSEL);
delayms(10);
Plot_Init(&g_plot_temp, -10, 40, "Temp (C)", 0);
Plot_Init(&g_plot_rh, 0, 100, "RH", 0);
Plot_Init(&g_plot_mag, -400, 300, "uTesla", 1);
Plot_SetSpecialValue(&g_plot_mag, 9000, "OL");
Compass_Init(&g_compass);
g_left_display = DISPLAY_TEMP;
g_right_display = DISPLAY_COMPASS;
// Straight to sleep on boot:
g_go_to_sleep = 1;
// Or not:
//g_go_to_sleep = 0;
//wakeup();
while(1) {
fillScreen(0x0);
if (g_go_to_sleep) {
// Write the empty back buffer to the screen:
eGFX_Dump(&eGFX_BackBuffer, &g_C12832A);
g_ignore_switches = 1;
// Sleep!
goToSleep();
// Processor has been woken up, restart clocks, etc.:
wakeup();
delayms(SW_DEBOUNCE_MS);
g_ignore_switches = 0;
}
switch (g_left_display) {
case DISPLAY_TEMP:
Plot_Draw(&eGFX_BackBuffer, &g_plot_temp, PLOT_LEFT);
break;
case DISPLAY_RH:
Plot_Draw(&eGFX_BackBuffer, &g_plot_rh, PLOT_LEFT);
break;
case DISPLAY_MAG:
Plot_Draw(&eGFX_BackBuffer, &g_plot_mag, PLOT_LEFT);
break;
case DISPLAY_COMPASS:
Compass_Draw(&eGFX_BackBuffer, &g_compass, COMPASS_LEFT);
break;
case DISPLAY_RANGE:
eGFX_DrawString(&eGFX_BackBuffer, "Range", 24, 1, &FONT_3_5_1BPP);
sprintf(str_buffer, "%0.2f cm", getRangeCentimeters());
eGFX_DrawString(&eGFX_BackBuffer, str_buffer, 24, 13, &FONT_3_5_1BPP);
break;
default:
break;
}
switch (g_right_display) {
case DISPLAY_TEMP:
Plot_Draw(&eGFX_BackBuffer, &g_plot_temp, PLOT_RIGHT);
break;
case DISPLAY_RH:
Plot_Draw(&eGFX_BackBuffer, &g_plot_rh, PLOT_RIGHT);
break;
case DISPLAY_MAG:
Plot_Draw(&eGFX_BackBuffer, &g_plot_mag, PLOT_RIGHT);
break;
case DISPLAY_COMPASS:
Compass_Draw(&eGFX_BackBuffer, &g_compass, COMPASS_RIGHT);
break;
case DISPLAY_RANGE:
eGFX_DrawString(&eGFX_BackBuffer, "Range", 88, 1, &FONT_3_5_1BPP);
sprintf(str_buffer, "%0.2f cm", getRangeCentimeters());
eGFX_DrawString(&eGFX_BackBuffer, str_buffer, 88, 13, &FONT_3_5_1BPP);
break;
default:
break;
}
eGFX_Dump(&eGFX_BackBuffer, &g_C12832A);
}
return 0 ;
}
/* Main loop */
int main()
{
/*Clear received array */
memset(&received[0], 0, sizeof(received));
/* Start the SCB UART, Timer and its interrupt */
ModbusUART_Start();
MessageReceived_StartEx(messageReceived_isr);
writeEnable_Write(0); // receive mode
I2C_MASTER_Start(); //Start communicating to HMC5883L
/* Start the encoder interrupt */
SpeedInterrupt_StartEx(speed_isr);
SpeedTimer_Start();
int i = 0;
CyGlobalIntEnable; /* comment this line to disable global interrupts. */
/* Setup Scaling factors for Modbus */
//mb.PIDScaler = 1000;
scaleModbusPIDConstants();
uint8 ida = 0;
HMC5883L_Init();
compassOnline = HMC5883L_testConnection();
if(compassOnline){
HMC5883L_initialize();
}
while(forever)
{
if(modbusMessage)
{
processMessage();
}
if(compassOnline)
{
HMC5883L_getHeading(&cx,&cy,&cz);
sx = (double)(cx) * scale * 10.0;
sy = (double)cy * scale * 10.0;
sz = (double)cz * scale * 10.0;
holdingReg[0] = (int16)sx;
holdingReg[1] = (int16)sy;
if((holdingReg[4] !=0) && (holdingReg[5] !=0))
{
//master has set offsets so it wants us to calculate the bearing here
offsetsx = ((double)((int16)holdingReg[4])) /10.0;
offsetsy = ((double)((int16)holdingReg[5])) /10.0;
bearing = atan2(((sy/10.0) + offsetsy), ((sx/10.0) + offsetsx));
unchangedBearing = bearing;
unchangedBearing = unchangedBearing*(180.0 / M_PI);//convert to degrees
holdingReg[11] = (int16)unchangedBearing; //store to -180 to 180 bearing;
if (bearing < 0)
bearing += 2 * M_PI;
bearing = bearing*(180.0 / M_PI);//convert to degrees
holdingReg[3] = (uint16)bearing*10;
pidSpeed = calculatePID(abs(bearing), abs(holdingReg[10]));
holdingReg[12] = (uint16) pidSpeed;
}
//We don't want to scale the PID contants every time as the floating point
//stuff is wasteful.
if(i < 1000){
i ++;
}
else{
scaleModbusPIDConstants();
i = 0;
}
if(speedInterruptFlag)
{
speedInterruptFlag = 0;
}
}
}
}
