int int_cmp(kv_t *k1, kv_t *k2)
{
assert(k1->t == k2->t);
assert(TKVINT == k1->t);
return _DI(k1) - _DI(k2);
}
void nOS_SwitchContext(void)
{
#if (NOS_CONFIG_MAX_UNSAFE_ISR_PRIO > 0)
nOS_StatusReg sr = _GetBASEPRI();
#endif
/* Request context switch */
*(volatile uint32_t *)0xE000ED04UL = 0x10000000UL;
/* Leave critical section */
#if (NOS_CONFIG_MAX_UNSAFE_ISR_PRIO > 0)
_SetBASEPRI(0);
#else
_EI();
#endif
_DSB();
_ISB();
_NOP();
/* Enter critical section */
#if (NOS_CONFIG_MAX_UNSAFE_ISR_PRIO > 0)
_SetBASEPRI(sr);
#else
_DI();
#endif
_DSB();
_ISB();
}
// send a packet to UART2
void udb_serial_send_packet( unsigned char *ucpData, int len )
{
// check and limit len, max send first 511 bytes
if ( len > 511 )
len = 511;
else ;
// check if still have data to transmit, if not we reset pointers to begin of buffer
if (iU2Head == iU2Tail)
{ _DI(); // have to make sure no interrupt
iU2Head = 0, iU2Tail = 0; // reset - also make it easy to see what's going on
_EI();
};
// first xfer data to private buffer
// if ( (iU2Head + len) < TX_BUF_LEN ) // case of data fits with one memcpy
// { memcpy( &U2TX_buffer[iU2Head], ucpData, len );
// iU1Head += len;
// } else { // will take multiple copies
// memcpy( &U2TX_buffer[iU2Head], ucpData, (TX_BUF_LEN - iU2Head) );
// ucpData += (TX_BUF_LEN - iU2Head); // next address
// len -= (TX_BUF_LEN - iU2Head); // what's left
// memcpy( &U2TX_buffer[0], ucpData, len );
// iU2Head = len;
// }
do { // when memcpy no longer fails this will be removed
U2TX_buffer[iU2Head] = *ucpData;
iU2Head++, ucpData++, len--;
if ( iU2Head >= TX_BUF_LEN )
iU2Head = 0;
else ;
} while (len > 0);
_U2TXIF = 1 ; // fire the tx interrupt
}
BYTE uart_getc (void)
{
BYTE d;
int i;
while (!RxFifo.ct) ; /* Wait while Rx FIFO empty */
i = RxFifo.ri; /* Get a byte from Rx FIFO */
d = RxFifo.buff[i++];
RxFifo.ri = i % BUFFER_SIZE;
_DI();
RxFifo.ct--;
_EI();
return d;
}
/* Put a byte into Tx2 FIFO */
void uart2_put (BYTE d)
{
int i;
while (Tx2Fifo.ct >= UART_BUFFER_SIZE) ; /* Wait while Tx FIFO full */
i = Tx2Fifo.wi; /* Push data into Tx FIFO */
Tx2Fifo.buff[i++] = d;
Tx2Fifo.wi = i % UART_BUFFER_SIZE;
_DI();
Tx2Fifo.ct++;
if (!Tx2Run) { /* If Tx is not running, generate 1st Tx interrupt */
Tx2Run = 1;
_U2TXIF = 1;
}
_EI();
}
/* Get a byte from Rx2 FIFO */
BYTE uart2_get (void)
{
BYTE d;
int i;
// FIX: CAN'T BLOCK HERE
while (!Rx2Fifo.ct) ; /* Wait while Rx FIFO empty */
i = Rx2Fifo.ri; /* Pop a byte from Rx FIFO */
d = Rx2Fifo.buff[i++];
Rx2Fifo.ri = i % UART_BUFFER_SIZE;
_DI();
Rx2Fifo.ct--;
_EI();
return d;
}
BYTE uart_get (void)
{
BYTE d;
int n;
while (!RxFifo.count);
n = RxFifo.rptr;
d = RxFifo.buff[n];
RxFifo.rptr = (n + 1) % BUFFER_SIZE;
_DI();
RxFifo.count--;
_EI();
return d;
}
void __attribute__((__interrupt__,__no_auto_psv__)) _T1Interrupt(void)
// excute whatever needs to run in the background, once every 0.5 seconds
{
// interrupt_save_extended_state ;
_T1IF = 0 ; // clear the interrupt
indicate_loading_inter ;
#if ( BOARD_TYPE == ASPG_BOARD )
if ( ++timer1_counts < TMR1_CNTS ) // actually interrupts every 0.1 sec
return;
else timer1_counts = 0; // reset counter
#endif
// capture cpu_timer once per second.
if (skip_timer_reset )
{
// catch another 1/2 second in timer 5
skip_timer_reset = 0;
}
else
{
#if ( BOARD_TYPE == ASPG_BOARD )
_DI();
cpu_counter += TMR5 ; // add last bit + calc %
cpu_timer = (int)((cpu_counter / CPU_LOAD_PERCENT));
old_cpu_counter = cpu_counter;
cpu_counter = 0; // clear it after
_EI();
#else
cpu_timer = TMR5 ;
#endif
T5CONbits.TON = 0 ; // turn off timer 5
TMR5 = 0 ; // reset timer 5 to 0
T5CONbits.TON = 1 ; // turn on timer 5
timer_5_on = 1;
skip_timer_reset = 1;
}
udb_background_callback_periodic() ;
// interrupt_restore_extended_state ;
return ;
}
void uart_putc (BYTE d)
{
int i;
while (TxFifo.ct >= BUFFER_SIZE) ; /* Wait while Tx FIFO is full */
i = TxFifo.wi; /* Put a data into the Tx FIFO */
TxFifo.buff[i++] = d;
TxFifo.wi = i % BUFFER_SIZE;
_DI();
TxFifo.ct++;
if (!TxRun) { /* If transmission sequense is not running, start the tx sequense */
TxRun = 1;
_U1TXIF = 1; /* Force trigger Tx interrupt */
}
_EI();
}
void uart_put (BYTE d)
{
int n;
while (TxFifo.count >= BUFFER_SIZE);
n = TxFifo.wptr;
TxFifo.buff[n] = d;
TxFifo.wptr = (n + 1) % BUFFER_SIZE;
_DI();
TxFifo.count++;
if (!TxRun) {
TxRun = 1;
_U1TXIF = 1;
}
_EI();
}
// Called at 40Hz
void udb_servo_callback_prepare_outputs(void)
{
//#if (MAG_YAW_DRIFT == 1)
// This is a simple counter to do stuff at 4hz
// Approximate time passing between each telemetry line, even though
// we may not have new GPS time data each time through.
/* if (tow.WW > 0) */ tow.WW += 25 ;
dcm_fourHertzCounter++ ;
if ( dcm_fourHertzCounter >= 10 )
{
rxMagnetometer() ; // now actually all interrupt driven but this will kickstart
rxAccel() ; // now actually all interrupt driven but this will kickstart
dcm_fourHertzCounter = 0 ;
}
//#endif
int iNumSamples;
int iIndex;
iAnalog_Tail = iAnalog_Head&0x3f; // should actually be iAnalog_Head
udb_setDSPLibInUse(true) ;
if ( iAnalog_Tail > 10 )
{
MDSFIR( iAnalog_Tail, &AD1_Filt[1][1][0], &AD1_Filt[0][1][0], &filter_aspgFilterX);
MDSFIR( iAnalog_Tail, &AD1_Filt[1][2][0], &AD1_Filt[0][2][0], &filter_aspgFilterY);
MDSFIR( iAnalog_Tail, &AD1_Filt[1][3][0], &AD1_Filt[0][3][0], &filter_aspgFilterZ);
_DI(); // have to run this int disabled - should be fast
iNumSamples = iAnalog_Head - iAnalog_Tail; // get any during FIR ?
if ( iNumSamples ) // copy those samples to begin of buffer
{
for ( iIndex = 0; iIndex < iNumSamples; iIndex++ )
{
AD1_Filt[0][1][iIndex] = AD1_Filt[0][1][iAnalog_Tail + iIndex];
AD1_Filt[0][2][iIndex] = AD1_Filt[0][2][iAnalog_Tail + iIndex];
AD1_Filt[0][3][iIndex] = AD1_Filt[0][3][iAnalog_Tail + iIndex];
};
iAnalog_Head = iIndex;
} else iAnalog_Head = 0;
_EI();
FLT_Value[1] = averageSample( &AD1_Filt[1][1][0], iAnalog_Tail );
FLT_Value[2] = averageSample( &AD1_Filt[1][2][0], iAnalog_Tail );
FLT_Value[3] = averageSample( &AD1_Filt[1][3][0], iAnalog_Tail );
// FLT_Value[1] = 0;
// FLT_Value[2] = 0;
// FLT_Value[3] = 0;
lastGyroSamples = iAnalog_Tail;
};
iI2C_Tail = iI2C_Head&0x3f; // should actually be iI2C_Head
if ( iI2C_Tail > 10 )
{
MDSFIR( iI2C_Tail, &AD1_Filt[1][4][0], &AD1_Filt[0][4][0], &filter_aspg_I2CX_Filter);
MDSFIR( iI2C_Tail, &AD1_Filt[1][5][0], &AD1_Filt[0][5][0], &filter_aspg_I2CY_Filter);
MDSFIR( iI2C_Tail, &AD1_Filt[1][6][0], &AD1_Filt[0][6][0], &filter_aspg_I2CZ_Filter);
_DI(); // have to run this int disabled - should be fast
iNumSamples = iI2C_Head - iI2C_Tail; // get any during FIR ?
if ( iNumSamples ) // copy those samples to begin of buffer
{
for ( iIndex = 0; iIndex < iNumSamples; iIndex++ )
{
AD1_Filt[0][4][iIndex] = AD1_Filt[0][4][iI2C_Tail + iIndex];
AD1_Filt[0][5][iIndex] = AD1_Filt[0][5][iI2C_Tail + iIndex];
AD1_Filt[0][6][iIndex] = AD1_Filt[0][6][iI2C_Tail + iIndex];
};
iI2C_Head = iIndex;
} else iI2C_Head = 0;
_EI();
FLT_Value[4] = averageSample( &AD1_Filt[1][4][0], iI2C_Tail );
FLT_Value[5] = averageSample( &AD1_Filt[1][5][0], iI2C_Tail );
FLT_Value[6] = averageSample( &AD1_Filt[1][6][0], iI2C_Tail );
lastAccelSamples = iI2C_Tail;
};
udb_setDSPLibInUse(false) ;
// udb_xaccel.value = udb_xaccel.value + (( (udb_xaccel.input>>1) - (udb_xaccel.value>>1) )>> FILTERSHIFT ) ;
// udb_xrate.value = udb_xrate.value + (( (udb_xrate.input>>1) - (udb_xrate.value>>1) )>> FILTERSHIFT ) ;
// udb_yaccel.value = udb_yaccel.value + (( ( udb_yaccel.input>>1) - (udb_yaccel.value>>1) )>> FILTERSHIFT ) ;
// udb_yrate.value = udb_yrate.value + (( (udb_yrate.input>>1) - (udb_yrate.value>>1) )>> FILTERSHIFT ) ;
// udb_zaccel.value = udb_zaccel.value + (( (udb_zaccel.input>>1) - (udb_zaccel.value>>1) )>> FILTERSHIFT ) ;
// this is the spot to add per channel analog scaling
analog_pin( (int)FLT_Value[gyro_x], &DIO[31] ); // see radioIn_aspg.c for maping
analog_pin( (int)FLT_Value[gyro_y], &DIO[32] ); // see radioIn_aspg.c for maping
analog_pin( (int)FLT_Value[gyro_z], &DIO[33] ); // see radioIn_aspg.c for maping
// udb_xrate.value = FLT_Value[gyro_x];
// udb_yrate.value = FLT_Value[gyro_y];
// udb_zrate.value = FLT_Value[gyro_z];
udb_xrate.value = DIO[31].qValue;
udb_yrate.value = DIO[32].qValue;
udb_zrate.value = DIO[33].qValue;
// this is the spot to add per channel analog scaling
analog_pin( (int)FLT_Value[accel_x]*2, &DIO[34] ); // see radioIn_aspg.c for maping
analog_pin( (int)FLT_Value[accel_y]*2, &DIO[35] ); // see radioIn_aspg.c for maping
analog_pin( (int)FLT_Value[accel_z]*2, &DIO[36] ); // see radioIn_aspg.c for maping
// udb_xaccel.value = FLT_Value[accel_x]*2;
// udb_yaccel.value = FLT_Value[accel_y]*2;
// udb_zaccel.value = FLT_Value[accel_z]*2;
udb_xaccel.value = DIO[34].qValue;
udb_yaccel.value = DIO[35].qValue;
udb_zaccel.value = DIO[36].qValue;
if (dcm_has_calibrated) {
dcm_run_imu_step() ;
}
dcm_servo_callback_prepare_outputs() ;
#if ( HILSIM == 1)
send_HILSIM_outputs() ;
#endif
return ;
}
void int_print(kv_t *kv)
{
assert (TKVINT == kv->t);
KVPRINT("%d", _DI(kv));
}