/** * Handle the timer interrupt by reading and parsing the status register * for the given timer. * * Return the channel number(s) in binary flags (0+2+3=>channel=12) * and a pointer to an array 4 long uint16_t value[4] */ int up_input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t value[4]) { uint32_t status; /* copy interrupt status */ status = rSR(timer); /* ack the interrupts we just read */ rSR(timer) = ~status; *channel = 0; /* which channel was this */ /* channel 1 */ if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { value[0] = input_pwm_decode(status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF), timer, 1); *channel |= (1 << 0); } /* channel 2 */ if (status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF)){ value[1] = input_pwm_decode(status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF), timer, 2); *channel |= (1 << 1); } /* channel 3 */ if (status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF)){ value[2] = input_pwm_decode(status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF), timer, 3); *channel |= (1 << 2); } /* channel 4 */ if (status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF)){ value[3] = input_pwm_decode(status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF), timer, 4); *channel |= (1 << 3); } return OK; }
/** * Handle the timer interrupt by reading and parsing the status register * for the given timer. * * Return the channel number(s) in binary flags (0+2+3=>channel=12) * and a pointer to an array 4 long uint16_t value[4] */ int up_input_pwm_timer_isr(uint8_t timer, uint8_t *channel, uint16_t value[4]) //input_pwm_timer_isr(int irq, void *context) { uint32_t status; // uint8_t timer; /* find the timer that caused the interrupt */ /* for (int i=0;i<INPUT_PWM_MAX_TIMERS;i++) if (input_pwm_timers[i].base != 0) if (input_pwm_timers[i].irq_vector == irq) { timer = i; break; } */ /* find out which channel caused the interupt */ /* grab the timer for latency tracking purposes */ //latency_actual = rCNT; /* copy interrupt status */ status = rSR(timer); /* ack the interrupts we just read */ rSR(timer) = ~status; /* was this a PPM edge? */ // if (status & (SR_INT_PPM | SR_OVF_PPM)) { // me->rc_decode(status); // } *channel = 0; /* which channel was this */ /* channel 1 */ if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { value[0] = input_pwm_decode(status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF), timer, 1); *channel |= (1 << 0); } /* channel 2 */ if (status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF)){ value[1] = input_pwm_decode(status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF), timer, 2); *channel |= (1 << 1); } /* channel 3 */ if (status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF)){ value[2] = input_pwm_decode(status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF), timer, 3); *channel |= (1 << 2); } /* channel 4 */ if (status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF)){ value[3] = input_pwm_decode(status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF), timer, 4); *channel |= (1 << 3); } // printf("count = %d, status=%d\n", rCCR1(timer),status); return OK; }
// GetElasticModuli() --------------------------------------------- void ManzariDafaliasRO::GetElasticModuli(const Vector& sigma, const double& en, double &K, double &G) // Calculates G, K { double p, pSR, Gmax, T, temp; Vector r(6), rSR(6); p = one3 * GetTrace(sigma); p = (p <= m_Pmin) ? m_Pmin : p; r = GetDevPart(sigma) / p; pSR = one3 * GetTrace(mSigmaSR); pSR = (pSR <= m_Pmin) ? m_Pmin : pSR; rSR = GetDevPart(mSigmaSR) / pSR; Gmax = m_B * m_P_atm / (0.3 + 0.7 * en * en) * sqrt(p / m_P_atm); if (mElastFlag == 0) { mIsFirstShear = true; T = 1.0; } else { mChi_r = sqrt(0.5 * DoubleDot2_2_Contr(r-rSR, r-rSR)); temp = m_kappa * (1.0 / m_a1 - 1); if (mIsFirstShear) T = 1 + temp * pow(mChi_r / mEta1, m_kappa - 1); else T = 1 + temp * pow(mChi_r / mEta1 / 2.0, m_kappa - 1); T = (T < (1.0+temp)) ? T : (1.0+temp); T = (T < 1.0) ? 1.0 : T; } G = Gmax / T; K = two3 * (1 + m_nu) / (1 - 2 * m_nu) * G; }
static int pwm_input_timer_isr(void) { uint16_t count = 0; for (unsigned i = 0; i < PWM_INPUT_MAX_TIMERS; i++) { status = rSR(i); //ack the interrupts we just read rSR(i) = ~status; if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { count = rCCR1(i); //printf("Captured on T%uC1 %u\n", i, count); for (unsigned j = 0; j < PWM_INPUT_MAX_CHANNELS; j++) { if ((pwm_input_channels[j].timer_channel == 1) && (pwm_input_channels[j].timer_index == i)) { rc[j] = count - rc_last[j]; rc_last[j] = count; if (rc[j] <= MAX_PULSEWIDTH) { ppm_buffer[j]=rc[j]; ppm_last_valid_decode = hrt_absolute_time(); } //printf("RC%u: %u\n", j, rc[j]); } } } if (status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF)) { count = rCCR2(i); //printf("Captured on T%uC2 %u\n", i, count); for (unsigned j = 0; j < PWM_INPUT_MAX_CHANNELS; j++) { if ((pwm_input_channels[j].timer_channel == 2) && (pwm_input_channels[j].timer_index == i)) { rc[j] = count - rc_last[j]; rc_last[j] = count; if (rc[j] <= MAX_PULSEWIDTH) { ppm_buffer[j]=rc[j]; ppm_last_valid_decode = hrt_absolute_time(); } //printf("RC%u: %u\n", j, rc[j]); } } } if (status & (GTIM_SR_CC3IF | GTIM_SR_CC3OF)) { count = rCCR3(i); //printf("Captured on T%uC3 %u\n", i, count); for (unsigned j = 0; j < PWM_INPUT_MAX_CHANNELS; j++) { if ((pwm_input_channels[j].timer_channel == 3) && (pwm_input_channels[j].timer_index == i)) { rc[j] = count - rc_last[j]; rc_last[j] = count; if (rc[j] <= MAX_PULSEWIDTH) { ppm_buffer[j]=rc[j]; ppm_last_valid_decode = hrt_absolute_time(); } //printf("RC%u: %u\n", j, rc[j]); } } } if (status & (GTIM_SR_CC4IF | GTIM_SR_CC4OF)) { count = rCCR4(i); //printf("Captured on T%uC3 %u\n", i, count); for (unsigned j = 0; j < PWM_INPUT_MAX_CHANNELS; j++) { if ((pwm_input_channels[j].timer_channel == 4) && (pwm_input_channels[j].timer_index == i)) { rc[j] = count - rc_last[j]; rc_last[j] = count; if (rc[j] <= MAX_PULSEWIDTH) { ppm_buffer[j]=rc[j]; ppm_last_valid_decode = hrt_absolute_time(); } //printf("RC%u: %u\n", j, rc[j]); } } } } return; }
static int tim_isr14(void) { status = rSR(0); //ack the interrupts we just read rSR(0) = ~status; if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { uint16_t count1 = rCCR1(0); //printf("Captured on RC1 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (GTIM_SR_CC1OF)) { printf("\nmissed an edge on RC1"); return; } /* how long since the last edge? */ rc1 = count1 - rc1_last; rc1_last = count1; if (rc1 <= MAX_PULSEWIDTH) rc_buffer[0]=rc1; //printf("RC1: %u\n", rc1); } status = rSR(1); //ack the interrupts we just read rSR(1) = ~status; if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { uint16_t count1 = rCCR1(1); //printf("Captured on RC3 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (GTIM_SR_CC1OF)) { printf("\nmissed an edge on RC3"); return; } /* how long since the last edge? */ rc3 = count1 - rc3_last; rc3_last = count1; if (rc3 <= MAX_PULSEWIDTH) rc_buffer[2]=rc3; //printf("RC3: %u\n", rc3); } if (status & (GTIM_SR_CC2IF | GTIM_SR_CC2OF)) { uint16_t count1 = rCCR2(1); //printf("Captured on RC4 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (GTIM_SR_CC2OF)) { printf("\nmissed an edge on RC4"); return; } /* how long since the last edge? */ rc4 = count1 - rc4_last; rc4_last = count1; if (rc4 <= MAX_PULSEWIDTH) rc_buffer[3]=rc4; //printf("RC4: %u\n", rc4); } status = rSR(2); //ack the interrupts we just read rSR(2) = ~status; if (status & (GTIM_SR_CC1IF | GTIM_SR_CC1OF)) { uint16_t count1 = rCCR1(2); //printf("Captured on RC2 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (GTIM_SR_CC1OF)) { printf("\nmissed an edge on RC2"); return; } /* how long since the last edge? */ rc2 = count1 - rc2_last; rc2_last = count1; if (rc2 <= MAX_PULSEWIDTH) rc_buffer[1]=rc2; //printf("RC2: %u\n", rc2); } status = rSR(3); //ack the interrupts we just read rSR(3) = ~status; if (status & (ATIM_SR_CC1IF | ATIM_SR_CC1OF)) { uint16_t count1 = rCCR1(3); //printf("Captured on RC6 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (ATIM_SR_CC1OF)) { printf("\nmissed an edge on RC6"); return; } */ /* how long since the last edge? */ rc6 = count1 - rc6_last; rc6_last = count1; if (rc6 <= MAX_PULSEWIDTH) rc_buffer[5]=rc6; //printf("RC6: %u\n", rc6); } if (status & (ATIM_SR_CC2IF | ATIM_SR_CC2OF)) { uint16_t count1 = rCCR2(3); //printf("Captured on RC5 %u\n", count1); /* if we missed an edge, we have to give up */ /* if (status & (ATIM_SR_CC2OF)) { printf("\nmissed an edge on RC5"); return; } /* how long since the last edge? */ rc5 = count1 - rc5_last; rc5_last = count1; if (rc5 <= MAX_PULSEWIDTH) rc_buffer[4]=rc5; //printf("RC5: %u\n", rc5); } return; }