void timer_notify(unsigned long data)
{
last_occured_alarm = last_alarm_set;
EnterMutex();
TimeDispatch();
LeaveMutex();
}
void timer_notify(sigval_t val)
{
if(gettimeofday(&last_sig,NULL)) {
perror("gettimeofday()");
}
EnterMutex();
TimeDispatch();
LeaveMutex();
// printf("getCurrentTime() return=%u\n", p.tv_usec);
}
/**
* 处理定时器的函数
* @brief 由于FreeRTOS系统定义中configUSE_TICK_HOOK为1，使用SYSTEM TICK。其调用
* 顺序为：
* SysTick_Handler(已被定义为xPortSysTickHandler)->xTaskIncrementTick->
* vApplicationTickHook，也就是此处需要自己定义的函数，用作CANOpen定时器
*
* 注意：在portmacro.h中，#define portTICK_PERIOD_MS ((TickType_t)1000/
* configTICK_RATE_HZ )，也就是说，间隔是1ms？
*/
void vApplicationTickHook(void)
{
/* CANOpen 定时器+1 */
TimerCounter_CAN++;
/* ALARM */
if(TimerCounter_CAN == TimerAlarm_CAN)
{
last_time_set = TimerCounter_CAN;
/* Call the time handler of the stack to adapt the elapsed time */
TimeDispatch();
/* This is important!!!!! */
}
}
/******************************************************************************
Interruptserviceroutine Timer 3 Compare 1 for the CAN timer
Pull in the timer counter value to represent the last time an alarm was set.
******************************************************************************/
void tim3_isr(void)
{
/* Clear interrrupt flag. */
if (timer_get_flag(TIM3, TIM_SR_CC1IF))
{
timer_clear_flag(TIM3, TIM_SR_CC1IF);
/* Reread to force the previous write before leaving (a side-effect of hardware pipelining)*/
timer_get_flag(TIM3, TIM_SR_CC1IF);
last_time_set = timer_get_counter(TIM3);
/* Call the time handler of the stack to adapt the elapsed time */
TimeDispatch();
}
}
/**
* Timer Task
*/
void timerloop_task_proc(void *arg)
{
int ret = 0;
getElapsedTime();
last_timeout_set = 0;
last_occured_alarm = last_time_read;
/* trigger first alarm */
SetAlarm(callback_od, 0, init_callback, 0, 0);
RTIME current_time;
RTIME real_alarm;
do{
rt_mutex_acquire(&condition_mutex, TM_INFINITE);
if(last_timeout_set == TIMEVAL_MAX)
{
ret = rt_cond_wait(
&timer_set,
&condition_mutex,
TM_INFINITE
); /* Then sleep until next message*/
rt_mutex_release(&condition_mutex);
}else{
current_time = rt_timer_read();
real_alarm = last_time_read + last_timeout_set;
ret = rt_cond_wait( /* sleep until next deadline */
&timer_set,
&condition_mutex,
(real_alarm - current_time)); /* else alarm consider expired */
if(ret == -ETIMEDOUT){
last_occured_alarm = real_alarm;
rt_mutex_release(&condition_mutex);
EnterMutex();
TimeDispatch();
LeaveMutex();
}else{
rt_mutex_release(&condition_mutex);
}
}
}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
if(exitall){
EnterMutex();
exitall(callback_od, 0);
LeaveMutex();
}
}
int
main(int argc, char** argv)
{
uint32_t led_colors = 0;
uint8_t at_parm_test[10];
unsigned once;
uint32_t i = 0;
uint8_t canPrescaler = 0;
uint8_t* uart_tx_packet = 0;
uint8_t* uart_rx_packet;
uint32_t old_loadcell_data;
uint16_t timeStep = 1;
clock_init();
pin_init();
led_init();
timers_init();
state_init();
uart_init();
// Set up UART2 for 115200 baud. There's no round() on the dsPICs, so we implement our own.
double brg = (double) 140000000 / 2.0 / 16.0 / 115200.0 - 1.0;
if (brg - floor(brg) >= 0.5) {
brg = ceil(brg);
}
else {
brg = floor(brg);
}
// Uart2Init (brg); // Init UART 2 as 115200 baud/s
loadcell_init();
loadcell_start();
led_rgb_off();
led_rgb_set(50, 0, 100);
can_state.init_return = RET_UNKNOWN;
if (can_init()) {
while (1);
}
timer_state.prev_systime = 0;
timer_state.systime = 0;
#ifdef CONF71
// Start Reading the int pin on IMU
mpuData.startData = 0;
if (IMU_Init(400000, 70000000) == 0) {
// imu_state.init_return = RET_OK;
mpuData.startData = 1;
}
else {
//imu_state.init_return = RET_ERROR;
}
#endif
for (;;) {
if (timer_state.systime != timer_state.prev_systime) {
timer_state.prev_systime = timer_state.systime;
//everything in here will be executed once every ms
//make sure that everything in here takes less than 1ms
//useful for checking state consistency, synchronization, watchdog...
if(timer_state.systime % 1000 == 1) {
LED_4 = 1;
}
led_update();
if (timer_state.systime % 10 == 1) {
IMU_GetQuaternion(quaterion);
QuaternionToYawPitchRoll(quaterion, ypr);
}
if (timer_state.systime % 5 == 1) {
IMU_normalizeData(&mpuData, &magData, &imuData);
// Run AHRS algorithm
IMU_UpdateAHRS (&imuData);
// Run IMU algorithm (does not use MAG data)
// IMU_UpdateIMU(&imuData);
//copy state to CAN dictionary
IMU_CopyOutput(&imuData, &mpuData, &magData);
}
/**
* CANFestival Loop
*/
if (can_state.init_return == RET_OK) {
can_process();
/**
* Sets CANFestival shared variables
* specific to Sensor Board
*/
can_push_state();
}
/**
* Blinking LED Loop
*/
if (timer_state.systime % 25 == 0) {
LED_1 = !LED_1;
}
}
else {
//untimed processes in main loop:
//executed as fast as possible
//these processes should NOT block the main loop
// LED_4 = mpuData.accelX > 0;
// LED_3 = mpuData.accelY > 0;
// LED_1 = mpuData.accelZ > 0;
// IMU_GetData();
IMU_CopyI2CData(&mpuData, &magData);
if (!T1CONbits.TON) {
RGB_RED = 0;
RGB_GREEN = RGB_BLUE = 1;
while (1);
}
if(can_flag){
TimeDispatch();
can_flag = 0;
}
uart_rx_packet = uart_rx_cur_packet();
if (uart_rx_packet != 0) {
uart_rx_packet_consumed();
}
/**
* Handles CAN transmission buffers
*/
if ((txreq_bitarray & 0b00000001) && !C1TR01CONbits.TXREQ0) {
C1TR01CONbits.TXREQ0 = 1;
txreq_bitarray = txreq_bitarray & 0b11111110;
}
if ((txreq_bitarray & 0b00000010) && !C1TR01CONbits.TXREQ1) {
C1TR01CONbits.TXREQ1 = 1;
txreq_bitarray = txreq_bitarray & 0b11111101;
}
if ((txreq_bitarray & 0b00000100) && !C1TR23CONbits.TXREQ2) {
C1TR23CONbits.TXREQ2 = 1;
txreq_bitarray = txreq_bitarray & 0b11111011;
}
if ((txreq_bitarray & 0b00001000) && !C1TR23CONbits.TXREQ3) {
C1TR23CONbits.TXREQ3 = 1;
txreq_bitarray = txreq_bitarray & 0b11110111;
}
if ((txreq_bitarray & 0b00010000) && !C1TR45CONbits.TXREQ4) {
C1TR45CONbits.TXREQ4 = 1;
txreq_bitarray = txreq_bitarray & 0b11101111;
}
if ((txreq_bitarray & 0b00100000) && !C1TR45CONbits.TXREQ5) {
C1TR45CONbits.TXREQ5 = 1;
txreq_bitarray = txreq_bitarray & 0b11011111;
}
if ((txreq_bitarray & 0b01000000) && !C1TR67CONbits.TXREQ6) {
C1TR67CONbits.TXREQ6 = 1;
txreq_bitarray = txreq_bitarray & 0b10111111;
}
}
}
return (EXIT_SUCCESS);
}
