/**
* 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;
}
