bool mhc5883_display_negative_test_result(MHC5883_t *structure)
{
if(!mhc5883_measurament_mode_set(structure, MHC5883_CONFIG_REG_A_MS_NEGATIVE_BIAS)) return false;
sys_delay(100);
signed short Xc = 0;
signed short Yc = 0;
signed short Zc = 0;
mhc5883_data_get(structure, &Xc, &Yc, &Zc);
UARTprintf(DebugCom, "Compass negative test:\n\rXc = %d, Yc = %d, Zc = %d\n\r", Xc, Yc, Zc);
if(!mhc5883_measurament_mode_set(structure, MHC5883_CONFIG_REG_A_MS_NORMAL)) return false;
sys_delay(100);
return true;
}
//菜单读键
//返回键值,特点是持续按下按键会自动连发
u32 menu_get_key()
{
u32 key;
while( 1 )
{
key = sys_get_key();
if( key )
{
sys_delay(200);
return key;
}
sys_delay(10);
}
}
void ss_sampl_H()
{
// Choose sampling method.
//#define SAMP_METHOD_SWING
#define SAMP_METHOD_SWING_S
// SWING mm
#if defined(SAMP_METHOD_SWING)
#define toR2 24+1 //20
#define toR3 12+2 //10
#define toR1 23 //15
#define toL1 23-2 //20
#define toL3 23 //20
#define toL2 12+3 //15
#define toC1 24+3 //20
#elif defined(SAMP_METHOD_SWING_S)
#define toR2 40
#define toR3 20
#define toR1 40
#define toL1 40
#define toL3 40
#define toL2 20
#define toC1 40
#else
#error Choose Sampling Method
#endif
led_off(PIO_LED_ALL);
led_on (PIO_LED_2 | PIO_LED_6 | PIO_LED_3);
// sys_delay(1000);
ss_on();
// show LEDs to find the center of block
int ss_diff;
while( !( get_key_state()==(PORT_CANCEL) || get_key_state()==(PORT_SELECT) ) )
{
ss_diff = ((int)ss_value[SS_L45]) - ((int)ss_value[SS_R45]);
led_off(PIO_LED_ALL);
if ( ss_diff > 10) led_on(PIO_LED_3);
else if ( ss_diff > 5) led_on(PIO_LED_4 | PIO_LED_1);
else if ( ss_diff < -10) led_on(PIO_LED_2);
else if ( ss_diff < -5 ) led_on(PIO_LED_5 | PIO_LED_0);
else led_on(PIO_LED_6);
}
// wait_key();
torque_on();
wait_key();
// ss_on();
led_count_down( 5, 1000 );
// debug
//#define DEBUG_SAMPL
disable_adj();
reset_spd_ptr();
// C1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_6 );
sys_delay(100); sampl_value( sC1 );
motor_start();
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R2
led_off( PIO_LED_0|PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_0|PIO_LED_5|PIO_LED_2 );
#if defined(SAMP_METHOD_SWING)
swing(toR2);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toR2);
#else
#error Choose Sampling Method
#endif
sampl_value( sR2 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R3
led_off( PIO_LED_0|PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_2 );
#if defined(SAMP_METHOD_SWING)
swing(toR3);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toR3);
#else
#error Choose Sampling Method
#endif
sampl_value( sR3 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_0 | PIO_LED_5);
#if defined(SAMP_METHOD_SWING)
swing(-toR1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toR1);
#else
#error Choose Sampling Method
#endif
sampl_value( sR1 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_1 | PIO_LED_4);
#if defined(SAMP_METHOD_SWING)
swing(-toL1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toL1);
#else
#error Choose Sampling Method
#endif
sampl_value( sL1 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L3
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_3);
#if defined(SAMP_METHOD_SWING)
swing(-toL3);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toL3);
#else
#error Choose Sampling Method
#endif
sampl_value( sL3 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L2
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_1|PIO_LED_3|PIO_LED_4 );
#if defined(SAMP_METHOD_SWING)
swing(toL2);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toL2);
#else
#error Choose Sampling Method
#endif
sampl_value( sL2 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// C1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_6 );
#if defined(SAMP_METHOD_SWING)
swing(toC1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toC1);
#else
#error Choose Sampling Method
#endif
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
motor_stop();
ss_off();
torque_off();
}
void ss_sampl()
{
while( 1 ) {
// Sampling Vertical //////////////////////////////////////
led_off(PIO_LED_ALL);
led_on (PIO_LED_0|PIO_LED_1|PIO_LED_5|PIO_LED_4);
if ( L_KEY == read_key() ) {
Sound_Select();
ss_sampl_V(); process_sampl_V();
save_sampl_value();
} else Sound_Beep();
// Sampling Horizonal //////////////////////////////////////
led_off(PIO_LED_ALL);
led_on (PIO_LED_2|PIO_LED_6|PIO_LED_3);
if ( L_KEY == read_key() ) {
Sound_Select();
ss_sampl_H(); process_sampl_V();
save_sampl_value();
} else Sound_Beep();
}
END_SEL:
}
void ss_sampl_V()
{
led_off(PIO_LED_ALL);
led_on (PIO_LED_0 | PIO_LED_1 | PIO_LED_5 | PIO_LED_4);
wait_key(); torque_on(); wait_key();
led_count_down( 5, 1000 );
disable_adj();
spd_ptr = 0;
spd_diff = 0;
reverse_LR();
ss_on(); sys_delay(300);
unsigned int ref_L;
unsigned int ref_R;
ref_L = ss_value[SS_L0];
ref_R = ss_value[SS_R0];
int i=0;
reset_spd_ptr();
spd_diff = 1;
motor_start();
while(i < SAMPL_VSTEP)
{
#define SAM_SPD 20 //25
if ( spd_ptr_L >= SAM_SPD ) {
spd_diff = 0;
equal_spd_ptr();
}
reset_steps();
while( steps == 0 );
ref_L = ss_value[SS_L0];
ref_R = ss_value[SS_R0];
sFL0[i] = (unsigned char)ref_L;
sFR0[i] = (unsigned char)ref_R;
i++;
}
ss_off();
spd_diff = -1;
while (spd_ptr_L > 0);
reset_spd_ptr();
motor_stop();
sys_delay(200);
reverse_LR();
torque_off();
}
/**
* @brief Power on the LCD.
* @param None
* @retval None
*/
bool ili9341_Init(void* pDisplay)
{
tDisplay* _pDisplay = (tDisplay *) pDisplay;
ILI9341_t *structure = (ILI9341_t *)_pDisplay->UserData;
gpio.out(structure->CS, 0);
gpio.out(structure->CS, 1);
gpio.out(structure->RW, 1);
gpio.out(structure->RD, 1);
ili9341_WriteReg(_pDisplay, 0xCA);
ili9341_WriteData(_pDisplay, 0xC3);
ili9341_WriteData(_pDisplay, 0x08);
ili9341_WriteData(_pDisplay, 0x50);
ili9341_WriteReg(_pDisplay, LCD_POWERB);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0xC1);
ili9341_WriteData(_pDisplay, 0x30);
ili9341_WriteReg(_pDisplay, LCD_POWER_SEQ);
ili9341_WriteData(_pDisplay, 0x64);
ili9341_WriteData(_pDisplay, 0x03);
ili9341_WriteData(_pDisplay, 0x12);
ili9341_WriteData(_pDisplay, 0x81);
ili9341_WriteReg(_pDisplay, LCD_DTCA);
ili9341_WriteData(_pDisplay, 0x85);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x78);
ili9341_WriteReg(_pDisplay, LCD_POWERA);
ili9341_WriteData(_pDisplay, 0x39);
ili9341_WriteData(_pDisplay, 0x2C);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x34);
ili9341_WriteData(_pDisplay, 0x02);
ili9341_WriteReg(_pDisplay, LCD_PRC);
ili9341_WriteData(_pDisplay, 0x20);
ili9341_WriteReg(_pDisplay, LCD_DTCB);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteReg(_pDisplay, LCD_FRMCTR1);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x1B);
ili9341_WriteReg(_pDisplay, LCD_DFC);
ili9341_WriteData(_pDisplay, 0x0A);
ili9341_WriteData(_pDisplay, 0xA2);
ili9341_WriteReg(_pDisplay, LCD_POWER1);
ili9341_WriteData(_pDisplay, 0x10);
ili9341_WriteReg(_pDisplay, LCD_POWER2);
ili9341_WriteData(_pDisplay, 0x10);
ili9341_WriteReg(_pDisplay, LCD_VCOM1);
ili9341_WriteData(_pDisplay, 0x45);
ili9341_WriteData(_pDisplay, 0x15);
ili9341_WriteReg(_pDisplay, LCD_VCOM2);
ili9341_WriteData(_pDisplay, 0x90);
ili9341_WriteReg(_pDisplay, LCD_MAC);
ili9341_WriteData(_pDisplay, 0xC8);
ili9341_WriteReg(_pDisplay, LCD_3GAMMA_EN);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteReg(_pDisplay, LCD_RGB_INTERFACE);
ili9341_WriteData(_pDisplay, 0xC2);
ili9341_WriteReg(_pDisplay, LCD_DFC);
ili9341_WriteData(_pDisplay, 0x0A);
ili9341_WriteData(_pDisplay, 0xA7);
ili9341_WriteData(_pDisplay, 0x27);
ili9341_WriteData(_pDisplay, 0x04);
/* Colomn address set */
ili9341_WriteReg(_pDisplay, LCD_COLUMN_ADDR);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0xEF);
/* Page address set */
ili9341_WriteReg(_pDisplay, LCD_PAGE_ADDR);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x01);
ili9341_WriteData(_pDisplay, 0x3F);
ili9341_WriteReg(_pDisplay, LCD_INTERFACE);
ili9341_WriteData(_pDisplay, 0x01);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x06);
ili9341_WriteReg(_pDisplay, LCD_GRAM);
sys_delay(200);
ili9341_WriteReg(_pDisplay, LCD_GAMMA);
ili9341_WriteData(_pDisplay, 0x01);
ili9341_WriteReg(_pDisplay, LCD_PGAMMA);
ili9341_WriteData(_pDisplay, 0x0F);
ili9341_WriteData(_pDisplay, 0x29);
ili9341_WriteData(_pDisplay, 0x24);
ili9341_WriteData(_pDisplay, 0x0C);
ili9341_WriteData(_pDisplay, 0x0E);
ili9341_WriteData(_pDisplay, 0x09);
ili9341_WriteData(_pDisplay, 0x4E);
ili9341_WriteData(_pDisplay, 0x78);
ili9341_WriteData(_pDisplay, 0x3C);
ili9341_WriteData(_pDisplay, 0x09);
ili9341_WriteData(_pDisplay, 0x13);
ili9341_WriteData(_pDisplay, 0x05);
ili9341_WriteData(_pDisplay, 0x17);
ili9341_WriteData(_pDisplay, 0x11);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteReg(_pDisplay, LCD_NGAMMA);
ili9341_WriteData(_pDisplay, 0x00);
ili9341_WriteData(_pDisplay, 0x16);
ili9341_WriteData(_pDisplay, 0x1B);
ili9341_WriteData(_pDisplay, 0x04);
ili9341_WriteData(_pDisplay, 0x11);
ili9341_WriteData(_pDisplay, 0x07);
ili9341_WriteData(_pDisplay, 0x31);
ili9341_WriteData(_pDisplay, 0x33);
ili9341_WriteData(_pDisplay, 0x42);
ili9341_WriteData(_pDisplay, 0x05);
ili9341_WriteData(_pDisplay, 0x0C);
ili9341_WriteData(_pDisplay, 0x0A);
ili9341_WriteData(_pDisplay, 0x28);
ili9341_WriteData(_pDisplay, 0x2F);
ili9341_WriteData(_pDisplay, 0x0F);
ili9341_WriteReg(_pDisplay, LCD_SLEEP_OUT);
sys_delay(200);
ili9341_WriteReg(_pDisplay, LCD_DISPLAY_ON);
/* GRAM start writing */
ili9341_WriteReg(_pDisplay, LCD_GRAM);
return true;
}
void trap_kernel_handler(struct user_context *user_ctx)
{
_debug("...... in %s, code = %p\n",
__func__, user_ctx->code & ~YALNIX_PREFIX);
switch (user_ctx->code) {
case YALNIX_FORK:
SET_RET(user_ctx, sys_fork(user_ctx));
break;
case YALNIX_EXEC:
sys_exec((char *)user_ctx->regs[0], (char **)user_ctx->regs[1],
user_ctx);
break;
case YALNIX_EXIT:
sys_exit(user_ctx->regs[0], user_ctx);
break;
case YALNIX_WAIT:
if (!FROM_USER_SPACE(user_ctx->regs[0])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_wait((int *)user_ctx->regs[0], user_ctx));
break;
case YALNIX_GETPID:
SET_RET(user_ctx, sys_getpid());
break;
case YALNIX_BRK:
SET_RET(user_ctx, sys_brk(user_ctx->regs[0]));
break;
case YALNIX_DELAY:
SET_RET(user_ctx, sys_delay(user_ctx->regs[0], user_ctx));
break;
case YALNIX_TTY_READ:
if (!FROM_USER_SPACE(user_ctx->regs[1])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_tty_read(user_ctx->regs[0],
(char *)user_ctx->regs[1],
user_ctx->regs[2],
user_ctx));
break;
case YALNIX_TTY_WRITE:
if (!FROM_USER_SPACE(user_ctx->regs[1])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_tty_write(user_ctx->regs[0],
(char *)user_ctx->regs[1],
user_ctx->regs[2],
user_ctx));
break;
case YALNIX_PIPE_INIT:
if (!FROM_USER_SPACE(user_ctx->regs[0])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_pipe_init((unsigned int *)
user_ctx->regs[0]));
break;
case YALNIX_PIPE_READ:
if (!FROM_USER_SPACE(user_ctx->regs[1])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_pipe_read(user_ctx->regs[0],
(char *)user_ctx->regs[1],
user_ctx->regs[2],
user_ctx));
break;
case YALNIX_PIPE_WRITE:
if (!FROM_USER_SPACE(user_ctx->regs[1])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_pipe_write(user_ctx->regs[0],
(char *)user_ctx->regs[1],
user_ctx->regs[2],
user_ctx));
break;
case YALNIX_LOCK_INIT:
if (!FROM_USER_SPACE(user_ctx->regs[0])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_lock_init((unsigned int *)
user_ctx->regs[0]));
break;
case YALNIX_LOCK_ACQUIRE:
SET_RET(user_ctx, sys_lock_acquire(user_ctx->regs[0],
user_ctx));
break;
case YALNIX_LOCK_RELEASE:
SET_RET(user_ctx, sys_lock_release(user_ctx->regs[0]));
break;
case YALNIX_CVAR_INIT:
if (!FROM_USER_SPACE(user_ctx->regs[0])) {
_error("Bad man! Please pass user space pointer!\n");
sys_exit(ERROR, user_ctx);
}
SET_RET(user_ctx, sys_cvar_init((unsigned int *)
user_ctx->regs[0]));
break;
case YALNIX_CVAR_WAIT:
SET_RET(user_ctx, sys_cvar_wait(user_ctx->regs[0],
user_ctx->regs[1],
user_ctx));
break;
case YALNIX_CVAR_SIGNAL:
SET_RET(user_ctx, sys_cvar_signal(user_ctx->regs[0]));
break;
case YALNIX_CVAR_BROADCAST:
SET_RET(user_ctx, sys_cvar_broadcast(user_ctx->regs[0]));
break;
case YALNIX_RECLAIM:
SET_RET(user_ctx, sys_reclaim(user_ctx->regs[0]));
break;
case YALNIX_CUSTOM_0:
SET_RET(user_ctx, sys_fork_share(user_ctx));
break;
}
return;
}
//等待按键清空
void menu_clear_key()
{
while( sys_get_key() )
sys_delay(10);
}