//
// Function: PrintDelayAndShow
//
// Description: My function to delay an show progress
//
// Arguments:
//
// Name Type Direction Description
// ----------- ----------- ----------- -----------------------------------
// Message char * IN String
/// TempMessage char * IN String to show while delay
// Delay int IN Milliseconds to delay
//
void PrintDelayAndShow( char *Message, char *TempMessage, int Delay )
{
int i, NumRepeat = 60;
FILE *pFile = GetErrorFilePointer(); //stdout;
if ( pFile != NULL )
{
if ( Delay > 1000 )
{
Delay = Delay / NumRepeat; //For show 60 pixels
fprintf( pFile, "\r%s %s", Message, TempMessage );
for ( i=NumRepeat; i>0; i-- )
{
DELAY_MS( Delay );
printf( "." );
}//end for
fprintf( pFile, "\r%s DONE\t\t\t\t\t\t\t\t\t\t\t\t\t\t\n", Message );
}//end if
}//end if
else
DELAY_MS( Delay + SHORT_DELAY );
}//end PrintDelayAndShow
/*****************************************************************************
�� �� �� : xfm20512_set_gain_direction
�������� : ����ʰ������ķ���
������� : int fd
unsigned int direction
������� : ��
�� �� ֵ :
���ú��� :
�������� :
����ʷ :
1.�� �� : 2016��6��16��
�� �� : zlg
������ : ����ɺ���
*****************************************************************************/
int xfm20512_set_gain_direction(unsigned int direction)
{
if (direction > 3)
{
return -1;
}
unsigned int data = 0x00001200 | ((direction & 0x3) << 16);
/* 1. ����ʰ������ */
if (i2c_write_proc(i2c_fileId, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1);
/* 2. ��ѯ����״̬ */
do
{
if (i2c_read_proc(i2c_fileId, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1);
} while (0x00030001 != data);
return 0;
}
void mpu6050_init() {
// printf("rspt: %x\n", MCM_ETBCC&6);
// SetIsr(NonMaskableInt_VECTORn, NMI_Handler);
// EnableIsr(NonMaskableInt_VECTORn);
i2c_init(I2C0, 400000);
DELAY_MS(100);
printf("init start\n");
mpu6050_short_init();
DELAY_MS(1000);
gyro_cal();
printf("init ends\n");
// float Rx = 0, raw_accel_angle = 0, sum = 0;
// for(int i=0; i<100; i++){
// float Rx = (((float) (adc_once(ADC0_SE17, ADC_10bit) - 525) * 3.3/ 1023))/ 0.8f;
//
// if(Rx > 1.0){
// Rx = 1.0;
// }else if(Rx < -1.0){
// Rx = -1.0;
// }
// raw_accel_angle = 90 - (acos(Rx) * 180 / 3.1415f - 90);
// sum += raw_accel_angle;
// }
// angle[0] = sum/100;
}
void cc2520_init(void) {
// ------------------------------------- GPIO ----------------------------------------//
// enable cc2520 related gpio and clks
RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
// RST -- PB8
GPIO_CONF_OUTPUT_PORT(B, 8, PUSH_PULL, 50);
// ---------------------------------- GPIO END ----------------------------------------//
// ------------------------------------ CC2520 ----------------------------------------//
CC2520_UNSELECT();
// reset
CC2520_DO_RST();
DELAY_MS(10);
CC2520_UNDO_RST();
DELAY_MS(10);
// ocs
cc2520_command(CMD_SXOSCON);
DELAY_MS(10);
//cc2520_wreg(FRMFILT0, 0x00); // no frame filtering
cc2520_command(CMD_SFLUSHRX);
cc2520_command(CMD_SFLUSHTX);
DELAY_MS(10);
// ---------------------------------- CC2520 END --------------------------------------//
}
void cloudMain() {
uprint("\r\n ************************ BOOT UP (CLOUD) ************************ \r\n");
configureAudio();
audioReset();
configureIRSend();
int sta = configureRadio(0x0A00, 0x0000111111111111);
uprint_int("Configured radio: ", sta);
char rxbuf[2];
int damageToSend, i;
configureCloudLighting();
DELAY_MS(400);
playSound(CS_BOOT);
DELAY_MS(CS_BOOT_LEN);
while (1) {
uprint("Beginning of main loop");
cloudLightingSetMode(ALGM_BLINK);
uprint("Waiting for hammer message");
radioGetMessage(rxbuf, 1);
uprint("Got message");
playSound(CS_FIRE);
// Don't wait for this sound
// Enter manual lighting mode
cloudLightingSetMode(ALGM_OFF);
cloudLightingSetAll(BRIGHTNESS_MAX);
cloudLightingUpdate();
damageToSend = rxbuf[0];
for (i = 0; i < damageToSend; i ++) {
uprint("Sending damage packet");
sendDamagePacket();
if ((i % 10) == 0) {
cloudLightingSetAll(BRIGHTNESS_MAX);
} else if ((i % 5) == 0) {
cloudLightingSetAll(50);
}
cloudLightingUpdate();
}
uprint("Sending DONE message");
radioSendMessage("DONE", 0x0A00);
}
}
/*******************************************************************************
* Function Name : motor_test()
* Description : 电机驱动测试
* Input : None
* Output : None
* Return : None
* Editor ：lyf
***************************************************************************/
void SERVO_test()
{
ftm_pwm_duty(FTM3, SERVO, 1100); //R
DELAY_MS(1000);
ftm_pwm_duty(FTM3, SERVO, 900);
DELAY_MS(1000);
}
/*******************************************************************************
* @fn SensorTagIO_blinkLed
*
* @brief Blinks a led 'n' times, duty-cycle 50-50
* @param led - led identifier
* @param nBlinks - number of blinks
*
* @return none
*/
void SensorTagIO_blinkLed(uint8_t led, uint8_t nBlinks)
{
uint8_t i;
for (i = 0; i<nBlinks; i++)
{
PIN_setOutputValue(hGpioPin, led, Board_LED_ON);
DELAY_MS(BLINK_DURATION);
PIN_setOutputValue(hGpioPin, led, Board_LED_OFF);
DELAY_MS(BLINK_DURATION);
}
}
/*!
* @brief 启动前进行检测（检测按键是否按下，按下则进入死循环，防止进入 main 函数,松开按键后再按下则进行解锁）
* @since v5.0
* @note 此函数可避免 下载口在 main 前 复用了导致无法下载等问题
*/
void start_check()
{
#if 0
uint8 flag = 0;
key_init(KEY_A);
if(key_check(KEY_A) == KEY_DOWN )
{
//按键按下
led_init(LED0);
while(1)
{
if(key_check(KEY_A) == KEY_UP)
{
flag = 1;
}
led_turn(LED0);
printf("\n进入死循环等待！");
DELAY_MS(500); //此处的延时，可起到 按键 消抖的功能
//如果 送开按键后，再按下 按键，则 进行解锁 操作
if((flag == 1) && (key_check(KEY_A) == KEY_DOWN ))
{
while(key_check(KEY_A) == KEY_DOWN) //等待按键 弹起
{
led_turn(LED0);
DELAY_MS(100); //此处的延时，可起到 按键 消抖的功能
}
while(key_check(KEY_A) == KEY_UP) //等待按键 弹起后 再 进行解锁
{
led_turn(LED0);
DELAY_MS(50); //此处的延时，可起到 按键 消抖的功能
}
printf("\n进行解锁操作，请重新刷入固件");
//解锁单片机
flash_init();
DELAY_MS(100);
flash_erase_sector(0); //擦除扇区 0 (解锁)
NVIC_SystemReset(); //复位单片机
}
}
}
#endif
}
/*********************************************************************
* @fn sensorTaskFxn
*
* @brief The task loop of the humidity readout task
*
* @return none
*/
static void sensorTaskFxn(UArg a0, UArg a1)
{
// Register task with BLE stack
ICall_registerApp(&sensorSelfEntity, &sensorSem);
// Deactivate task (active only when measurement is enabled)
Task_setPri(Task_handle(&sensorTask), -1);
// Task loop
while (true)
{
if (sensorConfig == ST_CFG_SENSOR_ENABLE)
{
uint8_t data[MS5607_DATA_SIZE];
sensorMs5607Read(data);
// int32_t temp;
// uint32_t press;
// bool success;
//
// // Readout
// SensorBmp280_enable(true);
// DELAY_MS(SENSOR_FSM_PERIOD);
// success = SensorBmp280_read(data);
// SensorBmp280_enable(false);
//
// // Processing
// if (success)
// {
// SensorBmp280_convert(data,&temp,&press);
//
// data[2] = (temp >> 16) & 0xFF;
// data[1] = (temp >> 8) & 0xFF;
// data[0] = temp & 0xFF;
//
// data[5] = (press >> 16) & 0xFF;
// data[4] = (press >> 8) & 0xFF;
// data[3] = press & 0xFF;
// }
// Send data
Barometer_setParameter(SENSOR_DATA, SENSOR_DATA_LEN, data);
DELAY_MS(sensorPeriod - SENSOR_FSM_PERIOD);
}
else
{
DELAY_MS(SENSOR_DEFAULT_PERIOD);
}
}
}
void radio_init(uint8_t * rxbuf, uint8_t rxbufsz)
{
trx_regval_t status;
/* init cpu peripherals and global IRQ enable */
radiostatus.rxframe = rxbuf;
radiostatus.rxframesz = rxbufsz;
//trx_set_irq_handler(radio_irq_handler);
/* transceiver initialization */
TRX_RESET_LOW();
TRX_SLPTR_LOW();
DELAY_US(TRX_RESET_TIME_US);
#if defined(CUSTOM_RESET_TIME_MS)
DELAY_MS(CUSTOM_RESET_TIME_MS);
#endif
TRX_RESET_HIGH();
/* disable IRQ and clear any pending IRQs */
trx_reg_write(RG_IRQ_MASK, 0);
trx_reg_read(RG_IRQ_STATUS);
trx_bit_write(SR_TRX_CMD, CMD_TRX_OFF);
DELAY_US(510);
status = trx_bit_read(SR_TRX_STATUS);
if (status != TRX_OFF)
{
radio_error(STATE_SET_FAILED);
}
trx_bit_write(SR_TX_AUTO_CRC_ON, 1);
trx_reg_write(RG_IRQ_MASK, TRX_IRQ_RX_START | TRX_IRQ_RX_END | TRX_IRQ_TX_END);
radiostatus.state = STATE_OFF;
radiostatus.idle_state = STATE_OFF;
}
static void
iec_reset(bool forever)
{
DEBUGF(DBG_ALL, "reset\n");
iec_release(IO_DATA | IO_ATN | IO_CLK);
/*
* Hold the device in reset a while. 20 ms was too short and it didn't
* fully reset (e.g., motor did not run). Nate checked with a scope
* and his 1541-B grabs DATA exactly 25 ms after RESET goes active.
* 30 ms seems good. It takes about 1.2 seconds before the drive answers
* by grabbing DATA.
*
* There is a small glitch at 25 ms after grabbing RESET where RESET out
* goes inactive for 1 us. This corresponds with the drive grabbing CLK
* and DATA, and for about 40 ns, ATN also. Nate assumes this is
* crosstalk from the VIAs being setup by the 6502.
*/
iec_set(IO_RESET);
/*
* For one of Womo's drives a reset low hold time of 30 ms was too short.
* Therefore the value was increased to 100ms. The drive already worked
* with a value of 45 ms, but it was more than doubled to 100 ms to get a
* safety margin for even very worse cabling or input circuit conditions.
*/
DELAY_MS(100);
iec_release(IO_RESET);
wait_for_free_bus(forever);
}
void turn_servo_CCW_degrees(servoIDs id, float degrees) {
uint16_t u16_delayTime = (degrees/360) * CONT_COUNTER_FULL_ROTATION_TIME;
turn_servo_by_pulse(id, COUNTER_PULSE);
DELAY_MS(u16_delayTime);
turn_servo_by_pulse(id, NEUTRAL_PULSE);
}
void task_wav_play() {
printf("WAV TASK (ID %d) started.\n", CURRENT_TASK_ID);
__vectors.user[MFP_INT + MFP_GPI2 - USER_ISR_START] = &wav_isr;
TBDR = 29; // approx 15.9 khz out
TBCR = 0x1; // prescaler of 4
// wait for the fifo to clear of garbage
DELAY_MS(50);
wav_ptr = song_start;
wav_end = wav_ptr + song_len - 1;
AER |= (1 << 2);
IERB |= INTR_GPI2;
IMRB |= INTR_GPI2;
// prime the fifo
memcpy(IO_DEV3, wav_ptr, 512);
wav_ptr += 512;
printf("Wav Playback started.\n");
// wait for end of clip & loop.
while(1) {
if (wav_ptr >= wav_end) {
wav_ptr = song_start;
printf("Wav looping!\n");
} else
TIL311 = ((int)(wav_ptr - song_start) * (song_len/16000) / song_len);
yield();
}
}
static int
bnx2x_dev_start(struct rte_eth_dev *dev)
{
struct bnx2x_softc *sc = dev->data->dev_private;
int ret = 0;
PMD_INIT_FUNC_TRACE();
ret = bnx2x_init(sc);
if (ret) {
PMD_DRV_LOG(DEBUG, "bnx2x_init failed (%d)", ret);
return -1;
}
if (IS_PF(sc)) {
rte_intr_callback_register(&(dev->pci_dev->intr_handle),
bnx2x_interrupt_handler, (void *)dev);
if(rte_intr_enable(&(dev->pci_dev->intr_handle)))
PMD_DRV_LOG(ERR, "rte_intr_enable failed");
}
ret = bnx2x_dev_rx_init(dev);
if (ret != 0) {
PMD_DRV_LOG(DEBUG, "bnx2x_dev_rx_init returned error code");
return -3;
}
/* Print important adapter info for the user. */
bnx2x_print_adapter_info(sc);
DELAY_MS(2500);
return ret;
}
void SecUtil_CHexPrintBuffer(
const char *Prefix_p,
const char *Postfix,
const void *Buffer_p,
size_t BufferLength
)
{
char LineBuf[84];
const uint8 *Data_p = Buffer_p;
size_t n;
char EmptyString[] = "";
static const SecUtil_HexFormat_t HexFormat = { "0x", ", ", ", " };
if (Prefix_p == NULL)
Prefix_p = EmptyString;
if (Postfix == NULL)
Postfix = EmptyString;
for (n = 0; n < BufferLength; n += 16) {
(void)SecUtil_BufferToHex(Data_p + n, MIN(16, BufferLength - n),
&HexFormat,
LineBuf, sizeof(LineBuf));
DELAY_MS(5);
printf("%s%s%s", Prefix_p, LineBuf, Postfix);
}
}
Car::Car()
: m_leds{Led(0), Led(1), Led(2), Led(3)},
m_uart(3, 115200),
m_motor1(0), m_motor2(1),
#ifdef LIBSC_USE_CAMERA
m_cam(CAM_W, CAM_H),
#endif
m_accel(RXADC),
m_lcd(true),
m_start_button(0),
m_joystick(0),
m_balancevolt(LIBSC_VOLT)
{
#ifdef LIBSC_USE_UART
m_uart.StartReceive();
libutil::InitDefaultFwriteHandler(&m_uart);
#endif
#ifdef LIBSC_USE_CAMERA
while (!m_cam.Init())
{
LOG_E("Camera f****d up!!!!!");
DELAY_MS(250);
}
m_cam.ShootContinuously();
#endif
#ifdef LIBSC_USE_K60_ENCODERS
FTM_QUAD_Init(FTM1);
FTM_QUAD_Init(FTM2);
#endif
}
/**
* swCmdRegister
*
* Command SWAP register and wait for response
*
* 'dAddr' Device address
* 'rAddr' Register address
* 'rId' Register id
* 'value' New SWDATA value
*
* Return:
* pointer to the SWAP data received from the remote device
* NULL in case of lack of response
*/
SWDATA* swCmdRegister(byte dAddr, byte rAddr, byte rId, SWDATA *value)
{
unsigned long i = SWAP_DELAY_QRY_INF / 10;
// Expected response
expectedRegAddr = rAddr;
expectedRegId = rId;
if (swTransmitCmd(dAddr, rAddr, rId, value) == MRFI_TX_RESULT_FAILED)
return NULL;
waitForSwapInfo = TRUE;
//Wait up to 500 msec for the response
while(waitForSwapInfo)
{
DELAY_MS(10);
if (i-- == 0)
{
waitForSwapInfo = FALSE;
return NULL;
}
}
return &glblSwapData;
}
/* Text a phone number */
void gsmText(char *number, char *msg) {
/* Store the beginning and ending sequences */
char *prefix = "AT+CMGS=\"";
char *suffix = "\"\r\n";
/* Allocate space and then store concatenated text command */
size_t len1 = strlen(prefix);
size_t len2 = strlen(number);
size_t len3 = strlen(suffix);
char *s = malloc(len1 + len2 + len3 + 1);
memcpy(s, prefix, len1);
memcpy(s + len1, number, len2);
memcpy(s + len1 + len2, suffix, len3 + 1);
/* Set GSM module to text mode */
gsmSendCommand("AT+CMGF=1\r");
/* Set the number */
gsmSendCommand(s);
/* Delay to allow GSM module to get ready */
DELAY_MS(100);
/* Set the message */
gsmSendCommand(msg);
/* Send Text */
while(BusyUART1());
WriteUART1((unsigned int)CTRL_Z);
/* Free memory */
free(s);
}
int main()
{
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
Clock_Init(0,0,0,12); // 12M DCO
TimerB_ConMode_Init(2, 4); // 3M的持续计数模式.
LCD5110_Init();
LCD5110_Write_String(0,0,LCD_BUFFER1);
Drive_UTL_Init();
_EINT();
while(1) //间隔1s 进行一次超声波测距
{
ULT_TRIG1;
DELAY_US(30);
ULT_TRIG0;
DELAY_MS(300);
Drive_UTL_DISTANCE();
UTL_START_TIME = 0;
LCD5110_Long2Char(UTL_BUFFER);
LCD5110_Write_String(0,1,0);
UTL_FLAG = 0;
}
}
/**
* swTransmit
*
* Transmit SWAP packet
*
* 'packet' Pointer to the SWAP packet to be sent
*
* Return:
* MRFI_TX_RESULT_SUCCESS - transmit succeeded
* MRFI_TX_RESULT_FAILED - transmit failed because CCA failed
*/
static byte swTransmit(SWPACKET *swPacket)
{
mrfiPacket_t mPacket;
int i;
byte ret;
mPacket.frame[0] = swPacket->value.length + SWAP_DATA_HEAD_LEN;
mPacket.frame[1] = swPacket->destAddr;
mPacket.frame[2] = swPacket->srcAddr;
mPacket.frame[3] = (swPacket->hop << 4) & 0xF0;
mPacket.frame[3] |= swPacket->security & 0x0F;
mPacket.frame[4] = swPacket->nonce;
mPacket.frame[5] = swPacket->function;
mPacket.frame[6] = swPacket->regAddr;
mPacket.frame[7] = swPacket->regId;
for(i=0 ; i<swPacket->value.length ; i++)
mPacket.frame[8+i] = swPacket->value.data[i];
i = SWAP_NB_TX_TRIES;
while ((ret = MRFI_Transmit(&mPacket, SWAP_TX_METHOD)) == MRFI_TX_RESULT_FAILED && i > 0)
{
i--;
DELAY_MS(SWAP_DY_TX_TRIES);
}
return ret;
}
void TEST_LightMCU() {
char rx[10];
unsigned int i, r, g, b, final;
while (1) {
uprint("l to loop, other for special");
rx[0] = uart1Rx();
if (rx[0] == 'l') {
for (i = 0; i < 192; i ++) {
uprint_int("Sending ", i);
sendLightMCU(i);
DELAY_MS(50);
}
} else {
uprint("Enter r 0-4: ");
uart1_gets(rx, 10);
r = atoi(rx);
uprint("Enter g 0-4: ");
uart1_gets(rx, 10);
g = atoi(rx);
uprint("Enter b 0-4: ");
uart1_gets(rx, 10);
b = atoi(rx);
final = 0b11000000 + (r << 4) + (g << 2) + b;
uprint_int("Sending ", final);
sendLightMCU(final);
}
}
void play_simon_infinitely() {
display_draw_number(SIMON_NUMBER);
#ifdef DEBUG_BUILD
printf("\n*** Playing Simon ***\n");
#endif
// Grab the Simon and calibrate the sensors
game_arm_pull_simon();
simon_hover_buttons();
DELAY_MS(PLATFORM_WAIT);
platform_rubiks();
twist_rubiks_clock();
DELAY_MS(PLATFORM_WAIT);
calibrate_sensors(); // I don't think we need calibration with the new transistors
// Stop playing if we exceed 5 rounds
while (1) {
#ifdef DEBUG_BUILD
printf("\nROUND %i\n", u8_roundNum);
#endif
// If it's the first round, hit the start button
if (u8_roundNum == 1) {
// Push the start button and record the colors
simon_push_button(START_SIMON_BUTTON);
record_colors(u8_roundNum);
u8_simonFinished = 0;
u16_milliSeconds = 0;
simon_hover_button(YELLOW_BUTTON);
} else {
// Record a certain number of colors
record_colors(u8_roundNum);
}
// Play back the buttons
DELAY_MS(500);
play_buttons(u8_roundNum);
u8_roundNum++;
}
// Let go of the simon and pull the arms back
twist_rubiks_counter();
DELAY_MS(PLATFORM_WAIT);
platform_up();
DELAY_MS(PLATFORM_WAIT);
simon_retract_buttons();
}
void main()
{
WDTCTL = WDTPW + WDTHOLD;
Clock_Init(0,0,0,CPU_FF); // 12M DCO
LCD5110_Init();
LCD5110_Write_String(0,0,LCD_MAIN_BUFFER1);
LCD5110_Write_String(0,1,LCD_MAIN_BUFFER2);
LCD5110_Write_String(0,2,LCD_MAIN_BUFFER3);
LCD5110_Write_String(0,3,LCD_MAIN_BUFFER4);
LCD5110_Write_String(0,4,LCD_MAIN_BUFFER5);
MPU6050_Init();
while(1)
{
MPU6050_AccelX();
LCD5110_Long2Char(MPU6050_AX);
LCD5110_Write_String(18,0,0);
MPU6050_AccelY();
LCD5110_Long2Char(MPU6050_AY);
LCD5110_Write_String(18,1,0);
MPU6050_AccelZ();
LCD5110_Long2Char(MPU6050_AZ);
LCD5110_Write_String(18,2,0);
MPU6050_Angle_XZX();
MPU6050_Angle_YZY();
LCD5110_Long2Char(MPU6050_ANGLE_XZX);
LCD5110_Write_String(18,3,0);
LCD5110_Long2Char(MPU6050_ANGLE_YZY);
LCD5110_Write_String(18,4,0);
/*
MPU6050_AnguX();
LCD5110_Long2Char(MPU6050_GX);
LCD5110_Write_String(18,3,0);
MPU6050_AnguY();
LCD5110_Long2Char(MPU6050_GY);
LCD5110_Write_String(18,4,0);
MPU6050_AnguZ();
LCD5110_Long2Char(MPU6050_GZ);
LCD5110_Write_String(18,5,0);
*/
DELAY_MS(500);
}
}
void ls_initialization()
{
unsigned int i;
ls_power_up();
for(i = 0; i < AVG; i++)
{
movingAvg[i] = 0;
creepAvg[i] = 0;
}
ls_set_scale(CALIBRATION_FACTOR);
DELAY_MS(10);
ls_tare(50);
DELAY_MS(10);
}
void B_main(void) {
uint16_t u16_count = 0;
while (1) {
printf("B: iteration %d.\n", u16_count++);
DELAY_MS(500);
gotoCoroutine(&scostate_main);
}
}
/*********************************************************************
* @fn SensorTagMov_processCharChangeEvt
*
* @brief SensorTag Movement event handling
*
* @param paramID - identifies which characteristic has changed
*
* @return none
*/
void SensorTagMov_processCharChangeEvt(uint8_t paramID)
{
uint16_t newCfg;
uint8_t newValue8;
switch (paramID)
{
case SENSOR_CONF:
if ((SensorTag_testResult() & SENSOR_MOV_TEST_BM) == 0)
{
mpuConfig = ST_CFG_ERROR;
}
if (mpuConfig != ST_CFG_ERROR)
{
Movement_getParameter(SENSOR_CONF, &newCfg);
if ((newCfg & MPU_AX_ALL) == ST_CFG_SENSOR_DISABLE)
{
// All axes off, turn off device power
mpuConfig = newCfg;
appStateSet(APP_STATE_OFF);
}
else
{
// Some axes on; power up and activate MPU
mpuConfig = newCfg;
appStateSet(APP_STATE_ACTIVE);
if (SensorMpu9250_powerIsOn())
{
DELAY_MS(5);
mpuConfig = newCfg | (SensorMpu9250_accReadRange() << 8);
}
}
Movement_setParameter(SENSOR_CONF, sizeof(mpuConfig), (uint8_t*)&mpuConfig);
}
else
{
// Make sure the previous characteristics value is restored
initCharacteristicValue(SENSOR_CONF, mpuConfig, sizeof(mpuConfig));
}
// Data initially zero
initCharacteristicValue(SENSOR_DATA, 0, SENSOR_DATA_LEN);
break;
case SENSOR_PERI:
Movement_getParameter(SENSOR_PERI, &newValue8);
sensorPeriod = newValue8 * SENSOR_PERIOD_RESOLUTION;
Util_rescheduleClock(&periodicClock,sensorPeriod);
break;
default:
// Should not get here
break;
}
}
int xfm20512_wakeup_reset(int fd)
{
unsigned int data = 0x00001100;
/* 1. Send enter wakeup command */
if (i2c_write_proc(fd, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1); // Delay 1ms at least
/* 2. Query command status */
do {
if (i2c_read_proc(fd, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1); // Delay 1ms at least
} while (0x00000001 != data);
return 0;
}
int xfm20512_exit_wakeup(int fd, unsigned int beam)
{
unsigned int data = 0x00001200 | ((beam & 0x3) << 16);
/* 1. Send exit wakeup command */
if (i2c_write_proc(fd, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1); // Delay 1ms at least
/* 2. Query command status */
do {
if (i2c_read_proc(fd, xfm20512_ADDR, 0x00, (unsigned char *)&data, sizeof(unsigned int)))
return -1;
DELAY_MS(1); // Delay 1ms at least
} while (0x00030001 != data);
return 0;
}
int main(void) {
configBasic(HELLO_MSG);
_LATB15 = 0;
// Initial value of RB15, at this line: 1, 0, X, Z.
while (1) {
DELAY_MS(250);
_LATB15 = !_LATB15;
}
}
/* Chip Reset */
static int mv88x201x_phy_reset(adapter_t *adapter)
{
u32 val;
(void) t1_tpi_read(adapter, A_ELMER0_GPO, &val);
val &= ~4;
(void) t1_tpi_write(adapter, A_ELMER0_GPO, val);
DELAY_MS(100);
(void) t1_tpi_write(adapter, A_ELMER0_GPO, val | 4);
DELAY_MS(1000);
/* Now lets enable the Laser. Delay 100us */
(void) t1_tpi_read(adapter, A_ELMER0_GPO, &val);
val |= 0x8000;
(void) t1_tpi_write(adapter, A_ELMER0_GPO, val);
DELAY_US(100);
return 0;
}
