void cyc_handle(intptr_t exinf)
{
static unsigned int count_ms = 0;
count_ms ++;
//每ms speed_task都需要运行
isig_sem(SPEED_SEM);
//每4ms sensors_task都需要运行
if (!(count_ms % 4))
isig_sem(SENSORS_SEM);
}
/*
* シリアルポートからの送信可能コールバック
*/
void
sio_irdy_snd(intptr_t exinf)
{
SPCB *p_spcb;
p_spcb = (SPCB *) exinf;
if (p_spcb->rcv_fc_chr != '\0') {
/*
* START/STOP を送信する．
*/
(void) sio_snd_chr(p_spcb->p_siopcb, p_spcb->rcv_fc_chr);
p_spcb->rcv_fc_chr = '\0';
}
else if (!(p_spcb->snd_stopped) && p_spcb->snd_count > 0U) {
/*
* 送信バッファ中から文字を取り出して送信する．
*/
(void) sio_snd_chr(p_spcb->p_siopcb,
p_spcb->p_spinib->snd_buffer[p_spcb->snd_read_ptr]);
INC_PTR(p_spcb->snd_read_ptr, p_spcb->p_spinib->snd_bufsz);
if (p_spcb->snd_count == p_spcb->p_spinib->snd_bufsz) {
if (isig_sem(p_spcb->p_spinib->snd_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->snd_count--;
}
else {
/*
* 送信すべき文字がない場合は，送信可能コールバックを禁止する．
*/
sio_dis_cbr(p_spcb->p_siopcb, SIO_RDY_SND);
}
}
/*
* シリアルポートからの送信可能コールバック
*/
void
sio_ierdy_snd(VP_INT exinf)
{
SPCB *spcb;
spcb = (SPCB *) exinf;
if (spcb->rcv_fc_chr != '\0') {
/*
* START/STOP を送信する．
*/
(void) sio_snd_chr(spcb->siopcb, spcb->rcv_fc_chr);
spcb->rcv_fc_chr = '\0';
}
else if (!(spcb->snd_stopped) && spcb->snd_count > 0) {
/*
* 送信バッファ中から文字を取り出して送信する．
*/
(void) sio_snd_chr(spcb->siopcb,
spcb->snd_buffer[spcb->snd_read_ptr]);
INC_PTR(spcb->snd_read_ptr);
if (spcb->snd_count == SERIAL_BUFSZ) {
_syscall(isig_sem(spcb->spinib->snd_semid));
}
spcb->snd_count--;
}
else {
/*
* 送信すべき文字がない場合は，送信可能コールバックを
* 禁止する．
*/
sio_dis_cbr(spcb->siopcb, SIO_ERDY_SND);
}
}
/**
* @fn void KzISRSpi(void)
* @brief SPI Ingterrupt Survice Routine
* @note KOBANZAME SDK kernel private function
*/
L1CODE
void KzISRSpi(void)
{
if( !rcv_snd() )
isig_sem( SPI_COMPLETE_SIG );
}
/*---------------------------------------------------------------------------*
* Routine: sys_sem_signal
*---------------------------------------------------------------------------*
* Description:
* Signals (releases) a semaphore
* Inputs:
* sys_sem_t sem -- Semaphore to signal
*---------------------------------------------------------------------------*/
void sys_sem_signal(sys_sem_t *data) {
ER ercd;
if (sns_ctx()) {
ercd = isig_sem(data->id);
} else {
ercd = sig_sem(data->id);
}
// if (osSemaphoreRelease(data->id) != osOK)
if (ercd != E_OK) {
// syslog(LOG_NOTICE, "Error with ercd=%d in sys_sem_signal.", ercd);
// mbed_die(); /* Can be called by ISR do not use printf */
return;
}
// syslog(LOG_NOTICE, " Semaphore (ID=%d) was released.", data->id);
}
void app_ter_btn_alm(intptr_t exinf) {
static bool_t wait_for_press = true;
static SYSTIM time;
if (global_brick_info.button_pressed[BRICK_BUTTON_BACK]) {
if (wait_for_press) {
time = _kernel_current_time;
wait_for_press = false;
} else {
if (_kernel_current_time - time >= 1000) { // Pressed for 1 second
isig_sem(APP_TER_SEM); // Terminate application
wait_for_press = true; // Reset this cyclic handler
istp_alm(APP_TER_BTN_ALM);
return; // Return to stop this alarm handler
}
}
} else {
wait_for_press = true;
}
ista_alm(APP_TER_BTN_ALM, 10);
}
/*
* シリアルポートからの受信通知コールバック
*/
void
sio_irdy_rcv(intptr_t exinf)
{
SPCB *p_spcb;
char c;
p_spcb = (SPCB *) exinf;
c = (char) sio_rcv_chr(p_spcb->p_siopcb);
if ((p_spcb->ioctl & IOCTL_FCSND) != 0U && c == FC_STOP) {
/*
* 送信を一時停止する．送信中の文字はそのまま送信する．
*/
p_spcb->snd_stopped = true;
}
else if (p_spcb->snd_stopped && (c == FC_START
|| (p_spcb->ioctl & IOCTL_FCANY) != 0U)) {
/*
* 送信を再開する．
*/
p_spcb->snd_stopped = false;
if (p_spcb->snd_count > 0U) {
c = p_spcb->p_spinib->snd_buffer[p_spcb->snd_read_ptr];
if (serial_snd_chr(p_spcb, c)) {
INC_PTR(p_spcb->snd_read_ptr, p_spcb->p_spinib->snd_bufsz);
if (p_spcb->snd_count == p_spcb->p_spinib->snd_bufsz) {
if (isig_sem(p_spcb->p_spinib->snd_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->snd_count--;
}
}
}
else if ((p_spcb->ioctl & IOCTL_FCSND) != 0U && c == FC_START) {
/*
* 送信に対してフロー制御している場合，START は捨てる．
*/
}
else if (p_spcb->rcv_count == p_spcb->p_spinib->rcv_bufsz) {
/*
* バッファフルの場合，受信した文字を捨てる．
*/
}
else {
/*
* 受信した文字を受信バッファに入れる．
*/
p_spcb->p_spinib->rcv_buffer[p_spcb->rcv_write_ptr] = c;
INC_PTR(p_spcb->rcv_write_ptr, p_spcb->p_spinib->rcv_bufsz);
if (p_spcb->rcv_count == 0U) {
if (isig_sem(p_spcb->p_spinib->rcv_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->rcv_count++;
/*
* STOPを送信する．
*/
if ((p_spcb->ioctl & IOCTL_FCRCV) != 0U && !(p_spcb->rcv_stopped)
&& p_spcb->rcv_count
>= BUFCNT_STOP(p_spcb->p_spinib->rcv_bufsz)) {
if (!serial_snd_chr(p_spcb, FC_STOP)) {
p_spcb->rcv_fc_chr = FC_STOP;
}
p_spcb->rcv_stopped = true;
}
}
}
void
task1(intptr_t exinf)
{
ER ercd;
check_point(1);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_CTX);
check_point(2);
ercd = loc_cpu();
check_ercd(ercd, E_OK);
ercd = pol_sem(SEM1);
check_ercd(ercd, E_CTX);
ercd = twai_sem(SEM1, TMO_POL);
check_ercd(ercd, E_CTX);
ercd = unl_cpu();
check_ercd(ercd, E_OK);
check_point(3);
ercd = dis_dsp();
check_ercd(ercd, E_OK);
ercd = pol_sem(SEM1);
check_ercd(ercd, E_OK);
ercd = twai_sem(SEM1, TMO_POL);
check_ercd(ercd, E_CTX);
ercd = ena_dsp();
check_ercd(ercd, E_OK);
check_point(4);
ercd = chg_ipm(TMAX_INTPRI);
check_ercd(ercd, E_OK);
ercd = pol_sem(SEM1);
check_ercd(ercd, E_TMOUT);
ercd = twai_sem(SEM1, TMO_POL);
check_ercd(ercd, E_CTX);
ercd = chg_ipm(TIPM_ENAALL);
check_ercd(ercd, E_OK);
check_point(5);
ercd = twai_sem(SEM1, -2);
check_ercd(ercd, E_PAR);
ercd = twai_sem(SEM1, TMO_POL);
check_ercd(ercd, E_TMOUT);
check_point(6);
ercd = sta_alm(ALM1, 10);
check_ercd(ercd, E_OK);
check_point(7);
ercd = twai_sem(SEM1, TMO_FEVR);
check_ercd(ercd, E_OK);
check_point(11);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(12);
ercd = wai_sem(SEM1);
check_ercd(ercd, E_OK);
check_point(15);
ercd = wai_sem(SEM1);
check_ercd(ercd, E_OK);
check_point(19);
ercd = wai_sem(SEM1);
check_ercd(ercd, E_OK);
check_point(23);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(25);
ercd = wai_sem(SEM1);
check_ercd(ercd, E_OK);
check_point(30);
ercd = sta_alm(ALM1, 10);
check_ercd(ercd, E_OK);
check_point(31);
ercd = twai_sem(SEM1, 100);
check_ercd(ercd, E_RLWAI);
check_point(33);
ercd = sta_alm(ALM1, 100);
check_ercd(ercd, E_OK);
check_point(34);
ercd = twai_sem(SEM1, 10);
check_ercd(ercd, E_TMOUT);
check_point(35);
ercd = stp_alm(ALM1);
check_ercd(ercd, E_OK);
check_finish(36);
check_point(0);
}
void
alarm1_handler(intptr_t exinf)
{
ER ercd;
switch (++alarm1_count) {
case 1:
check_point(8);
ercd = pol_sem(SEM1);
check_ercd(ercd, E_CTX);
ercd = twai_sem(SEM1, TMO_POL);
check_ercd(ercd, E_CTX);
check_point(9);
ercd = iloc_cpu();
check_ercd(ercd, E_OK);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_CTX);
ercd = iunl_cpu();
check_ercd(ercd, E_OK);
check_point(10);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_OK);
return;
check_point(0);
case 2:
check_point(14);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_OK);
signal_var();
return;
check_point(0);
case 3:
check_point(17);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_OK);
signal_var();
return;
check_point(0);
case 4:
check_point(21);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_OK);
signal_var();
return;
check_point(0);
case 5:
check_point(28);
ercd = isig_sem(SEM1);
check_ercd(ercd, E_OK);
signal_var();
return;
check_point(0);
case 6:
check_point(32);
ercd = irel_wai(TASK1);
check_ercd(ercd, E_OK);
return;
check_point(0);
}
check_point(0);
}
/*
* シリアルポートからの受信通知コールバック
*/
void
sio_ierdy_rcv(VP_INT exinf)
{
SPCB *spcb;
char c;
spcb = (SPCB *) exinf;
c = (char) sio_rcv_chr(spcb->siopcb);
if ((spcb->ioctl & IOCTL_FCSND) != 0 && c == FC_STOP) {
/*
* 送信を一時停止する．送信中の文字はそのまま送信する．
*/
spcb->snd_stopped = TRUE;
}
else if (spcb->snd_stopped && (c == FC_START
|| (spcb->ioctl & IOCTL_FCANY) != 0)) {
/*
* 送信を再開する．
*/
spcb->snd_stopped = FALSE;
if (spcb->snd_count > 0) {
c = spcb->snd_buffer[spcb->snd_read_ptr];
if (serial_snd_chr(spcb, c)) {
INC_PTR(spcb->snd_read_ptr);
if (spcb->snd_count == SERIAL_BUFSZ) {
_syscall(isig_sem(spcb->spinib
->snd_semid));
}
spcb->snd_count--;
}
}
}
else if ((spcb->ioctl & IOCTL_FCSND) != 0 && c == FC_START) {
/*
* 送信に対してフロー制御している場合，START は捨てる．
*/
}
else if (spcb->rcv_count == SERIAL_BUFSZ) {
/*
* バッファフルの場合，受信した文字を捨てる．
*/
}
else {
/*
* 受信した文字を受信バッファに入れる．
*/
spcb->rcv_buffer[spcb->rcv_write_ptr] = c;
INC_PTR(spcb->rcv_write_ptr);
if (spcb->rcv_count == 0) {
_syscall(isig_sem(spcb->spinib->rcv_semid));
}
spcb->rcv_count++;
/*
* STOP を送信する．
*/
if ((spcb->ioctl & IOCTL_FCRCV) != 0 && !(spcb->rcv_stopped)
&& (spcb->rcv_count >= BUFCNT_STOP)) {
if (!serial_snd_chr(spcb, FC_STOP)) {
spcb->rcv_fc_chr = FC_STOP;
}
spcb->rcv_stopped = TRUE;
}
}
}
