/* normally the tone frequency should be 31 to 4978, refer to piches.h */
void tone(uint8_t pin, uint16_t frequency, unsigned long duration)
{
static struct rt_timer timer2, timer3;
rt_timer_t timer;
RT_ASSERT(frequency * 2 < UINT16_MAX);
if(pin < 2 || pin > 3)
return;
if (pin == 2)
{
timer = &timer2;
}
else if (pin == 3)
{
timer = &timer3;
}
rt_timer_stop(timer);
rt_timer_init(timer, "pwmkill",
pwm_shutdown, (void*) pin,
rt_tick_from_millisecond(duration),
RT_TIMER_FLAG_ONE_SHOT);
TIM_config(pin, frequency);
pwmConfig(pin, UINT8_MAX / 2);
rt_timer_start(timer);
}
static void sleep_tick_adjust(uint32_t ms)
{
uint32_t diff;
diff = rt_tick_from_millisecond(ms);
rt_tick_set(rt_tick_get() + diff);
{
struct rt_thread *thread;
/* check time slice */
thread = rt_thread_self();
if (thread->remaining_tick <= diff)
{
/* change to initialized tick */
thread->remaining_tick = thread->init_tick;
/* yield */
rt_thread_yield();
}
else
{
thread->remaining_tick -= diff;
}
/* check timer */
rt_timer_check();
}
}
static rt_err_t rt_key_open(rt_device_t dev, rt_uint16_t oflag)
{
rt_timer_t timer;
timer = rt_timer_create("timer", timer_callback, &s_dev_key, rt_tick_from_millisecond(KEY_TIMER_INTERVAL), RT_TIMER_FLAG_PERIODIC/* | RT_TIMER_FLAG_SOFT_TIMER */);
RT_ASSERT(timer != RT_NULL);
rt_timer_start(timer);
return RT_EOK;
}
/// Wait until a Semaphore becomes available
int32_t osSemaphoreWait(osSemaphoreId semaphore_id, uint32_t millisec)
{
rt_tick_t ticks;
if (semaphore_id == RT_NULL)
return -1;
ticks = rt_tick_from_millisecond(millisec);
rt_sem_take(semaphore_id, ticks);
return semaphore_id->value;
}
void hm_heatcal_thread_entry(void* parameter)
{
rt_uint32_t event_set = 0;
float heat_used = 0;
cal_event = rt_event_create("H_cal", RT_IPC_FLAG_FIFO);
RT_ASSERT(cal_event);
while(1) {
if(rt_event_recv(cal_event, TDC_DATA_FULL_EVENT,
RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER,
&event_set)==RT_EOK) {
if((rt_mutex_take(temp_lock, rt_tick_from_millisecond(LOCK_TACK_WAIT_TIME_MS)) ||
(rt_mutex_take(tof_lock, rt_tick_from_millisecond(LOCK_TACK_WAIT_TIME_MS)))) != RT_EOK) {
rt_kprintf("TOF and temprature take lock error\n");
}
heat_used += do_heat_cal();
rt_mutex_release(tof_lock);
rt_mutex_release(temp_lock);
heat_print(heat_used);
}
}
}
/// Wait until a Mutex becomes available
osStatus osMutexWait(osMutexId mutex_id, uint32_t millisec)
{
rt_err_t result;
rt_tick_t ticks;
ticks = rt_tick_from_millisecond(millisec);
result = rt_mutex_take(mutex_id, ticks);
if (result == RT_EOK)
return osOK;
else
return osErrorOS;
}
/// Wait for Timeout (Time Delay)
osStatus osDelay(uint32_t millisec)
{
rt_err_t result;
rt_tick_t ticks;
ticks = rt_tick_from_millisecond(millisec);
result = rt_thread_delay(ticks);
if (result == RT_EOK)
return osOK;
else
return osErrorOS;
}
/// Start or restart timer
osStatus osTimerStart(osTimerId timer_id, uint32_t millisec)
{
rt_err_t result;
rt_tick_t ticks;
ticks = rt_tick_from_millisecond(millisec);
rt_timer_control(timer_id, RT_TIMER_CTRL_SET_TIME, &ticks);
result = rt_timer_start(timer_id);
if (result == RT_EOK)
return osOK;
else
return osErrorOS;
}
/// Wait for Signal, Message, Mail, or Timeout
osEvent osWait(uint32_t millisec)
{
rt_err_t result;
rt_tick_t ticks;
ticks = rt_tick_from_millisecond(millisec);
result = rt_thread_delay(ticks);
/*
if (result == RT_EOK)
return osOK;
else
return osErrorOS;
*/
}
int capture_jpeg(unsigned char *buf, int bufsz)
{
int len = bufsz;
FHADV_VIDEO_CHANNEL_t ch_main;
sdk_sensor_mem_init();
if (FHAdv_Video_Open() == FH_FAILURE)
return -1;
ch_main.channelId = 0;
ch_main.res = FHADV_VIDEO_RES_720P;
ch_main.fps = 25;
ch_main.bitrate = 1536;
ch_main.gop = 20;
ch_main.rcmode = FHADV_RC_CBR;
ch_main.cb = NULL;
if (FHAdv_Video_AddChannel(&ch_main) == FH_FAILURE)
return -1;
if (FHAdv_Video_Start(-1) == FH_FAILURE)
return -1;
/* wait for 100ms */
rt_thread_delay(rt_tick_from_millisecond(100));
if (FH_SUCCESS == FHAdv_Video_CaptureJpeg(0, (FH_ADDR)buf, &len))
{
rt_kprintf(
"INFO: Capture JPEG picture successfully, "
"the size is %d\n",
len);
}
else
{
len = 0;
}
FHAdv_Video_Stop(-1);
FHAdv_Video_Close();
return len;
}
/// Get a Message or Wait for a Message from a Queue
osEvent osMessageGet(osMessageQId queue_id, uint32_t millisec)
{
osEvent event;
rt_err_t result;
rt_tick_t ticks;
ticks = rt_tick_from_millisecond(millisec);
result = rt_mq_recv(queue_id, &event.value, 4, ticks);
if (result == RT_EOK)
{
event.status = osEventMessage;
}
else
{
event.status = osEventTimeout;
}
return event;
}
void hm_tdc_thread_entry(void* parameter)
{
rt_device_t tdc = RT_NULL;
rt_uint8_t tof_data_count = 0;
rt_uint8_t temp_data_count = 0;
rt_uint8_t loop_count = TOF_DATA_BUF_LEN/TEMP_DATA_BUF_LEN;
tdc = rt_device_find(HM_BOARD_TDC_NAME);
RT_ASSERT(tdc);
rt_device_open(tdc, RT_DEVICE_OFLAG_RDWR);
#if (HM_BOARD_UART_6 == 1)
tt_print_device = rt_device_find(TOF_TEMP_PRINT_DEVICE);
RT_ASSERT(tt_print_device);
rt_device_open(tt_print_device, RT_DEVICE_OFLAG_RDWR);
#endif
tof_lock = rt_mutex_create("L_tof", RT_IPC_FLAG_FIFO);
RT_ASSERT(tof_lock);
temp_lock = rt_mutex_create("L_temp", RT_IPC_FLAG_FIFO);
RT_ASSERT(temp_lock);
while(1) {
loop_count--;
{ /* TOF */
if(rt_mutex_take(tof_lock, rt_tick_from_millisecond(LOCK_TACK_WAIT_TIME_MS)) != RT_EOK) {
rt_kprintf("TOF take lock error\n");
continue;
}
else {
rt_device_control(tdc, SPI_TDC_GP21_CTRL_MEASURE_TOF2, &tof_data[tof_data_count].data);
tof_data[tof_data_count].time = rt_tick_get();
rt_mutex_release(tof_lock);
tof_data_count++;
}
}
if(loop_count == 0) { /* TEMP */
loop_count = TOF_DATA_BUF_LEN/TEMP_DATA_BUF_LEN;
if(rt_mutex_take(temp_lock, rt_tick_from_millisecond(LOCK_TACK_WAIT_TIME_MS)) != RT_EOK) {
rt_kprintf("temprature take lock error\n");
continue;
}
else {
rt_device_control(tdc, SPI_TDC_GP21_CTRL_MEASURE_TEMP, &temp_data[temp_data_count].data);
temp_data[temp_data_count].time = rt_tick_get();
rt_mutex_release(temp_lock);
temp_data_count++;
}
}
if((temp_data_count==TEMP_DATA_BUF_LEN) || (tof_data_count==TOF_DATA_BUF_LEN)) { /* Send event */
temp_data_count = 0;
tof_data_count = 0;
if(rt_event_send(cal_event, TDC_DATA_FULL_EVENT) != RT_EOK) {
rt_kprintf("TDC send event error\n");
}
}
rt_thread_delay(rt_tick_from_millisecond(TDC_SLEEP_TIME_MS));
}
}
/*==============================================================================================*/
void MPU6050_Init( void )
{
u8 MPU6050_Init_Data[6] = {
0x01, /* MPU6050_PWR_MGMT_1 */
0x03, /* MPU6050_CONFIG */
0x18, /* MPU6050_GYRO_CONFIG +-2000dps */
0x08, /* MPU6050_ACCEL_CONFIG +-4G */
0x32, /* MPU6050_INT_PIN_CFG */
0x00 /* MPU6050_USER_CTRL */
};
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_PWR_MGMT_1, MPU6050_Init_Data, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_CONFIG, MPU6050_Init_Data+1, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_GYRO_CONFIG, MPU6050_Init_Data+2, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_CONFIG, MPU6050_Init_Data+3, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_INT_PIN_CFG, MPU6050_Init_Data+4, 1); rt_thread_delay(rt_tick_from_millisecond(10));
I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_USER_CTRL, MPU6050_Init_Data+5, 1); rt_thread_delay(rt_tick_from_millisecond(10));
}
