| | |
|---|
| | | |
|---|
| | | #define NSH1 0x0001 |
|---|
| | | #define GP 0x0002 |
|---|
| | | //#define DEBUG_MODE |
|---|
| | | #define DEBUG_MODE |
|---|
| | | /* USER CODE END Includes */ |
|---|
| | | |
|---|
| | | /* Private typedef -----------------------------------------------------------*/ |
|---|
| | |
|---|
| | | extern float nomove_count; |
|---|
| | | float motor_keeptime; |
|---|
| | | uint8_t imu_enable,motor_enable; |
|---|
| | | uint8_t chongqi_num; |
|---|
| | | u16 GetLPTime(void) |
|---|
| | | { |
|---|
| | | u16 count=HAL_LPTIM_ReadCounter(&hlptim1); |
|---|
| | |
|---|
| | | hardware_pici= STMFLASH_ReadHalfWord(FLASH_HARDWARE_VERSION_MAP+2); |
|---|
| | | hardware_type= STMFLASH_ReadHalfWord(FLASH_HARDWARE_VERSION_MAP+4); |
|---|
| | | //deca_sleep(1000); |
|---|
| | | g_com_map[VERSION] = (2<<8)|7; |
|---|
| | | |
|---|
| | | g_com_map[VERSION] = (2<<8)|11; |
|---|
| | | // g_com_map[COM_INTERVAL]=200; |
|---|
| | | // g_com_map[DEV_ID]=0x7; |
|---|
| | | #ifdef DEBUG_MODE |
|---|
| | | // g_com_map[DEV_ROLE]=1; |
|---|
| | |
|---|
| | | // HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET); |
|---|
| | | // } |
|---|
| | | // LED_TR_BLINK; |
|---|
| | | chongqi_num=0; |
|---|
| | | __disable_irq(); |
|---|
| | | if(bat_percent>15) |
|---|
| | | { LED_TB_ON; |
|---|
| | |
|---|
| | | void Up_tagdisterror_list_Poll() |
|---|
| | | { |
|---|
| | | static uint8_t r=0; |
|---|
| | | static uint8_t t=0; |
|---|
| | | for(r=0;r<taglist_num;r++) |
|---|
| | | { |
|---|
| | | tagdisterror_list[r]++; |
|---|
| | | if(tagdisterror_list[r]>10) |
|---|
| | | { |
|---|
| | | tagdist_list[r]=0; |
|---|
| | | tagid_list[r]=0; |
|---|
| | | for(t=r;t<taglist_num;t++) |
|---|
| | | { |
|---|
| | | tagdist_list[t]=tagdist_list[t+1]; |
|---|
| | | tagid_list[t]=tagid_list[t+1]; |
|---|
| | | tagdisterror_list[t]=tagdisterror_list[t+1]; |
|---|
| | | taglist_num=taglist_num-1; |
|---|
| | | tagdisterror_list[r]=0; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | |
|---|
| | | static uint8_t i=0; |
|---|
| | | SystemClock_Config(); |
|---|
| | | g_start_send_flag=1; |
|---|
| | | chongqi_num++; |
|---|
| | | if(chongqi_num>20) |
|---|
| | | {NVIC_SystemReset();} |
|---|
| | | // current_slotnum++; |
|---|
| | | // SetNextPollTime(tagslotpos); |
|---|
| | | if(button_delay<1) |
|---|
| | | button_delay+=1/(float)tag_frequency; |
|---|
| | | nomove_count+=1/(float)tag_frequency; |
|---|
| | | Up_tagdisterror_list_Poll(); |
|---|
| | | //Up_tagdisterror_list_Poll(); |
|---|
| | | // memset(tagdist_list,0,taglist_num); |
|---|
| | | // memset(tagid_list,0,taglist_num); |
|---|
| | | // taglist_num=0; |
|---|
| | | TagListUpdate(); |
|---|
| | | bubble_sort(tagdist_list,taglist_num); |
|---|
| | | if(motor_keeptime>0) |
|---|
| | | { |
|---|
| | | motor_keeptime-=1/(float)tag_frequency; |
|---|
