void cmd_stack(void)
{
#if (RECORD_FREE_STACK_SPACE == 1)
printf("maxstack %x\r\n", maxstack);
printf("SP_start %x\r\n", SP_start());
printf("SP_limit %x\r\n", SP_limit());
printf("SP_current %x\r\n", SP_current());
printf("stack usage %u\r\n", maxstack - SP_start());
#else
printf("stack reporting disabled.\r\n");
#endif
}
void __attribute__((__interrupt__,__no_auto_psv__)) _DMA0Interrupt(void)
{
indicate_loading_inter;
interrupt_save_set_corcon;
#if (RECORD_FREE_STACK_SPACE == 1)
uint16_t stack = SP_current();
if (stack > maxstack)
{
maxstack = stack;
}
#endif
#if (HILSIM != 1)
int16_t *CurBuffer = (DmaBuffer == 0) ? BufferA : BufferB;
udb_xrate.input = CurBuffer[xrateBUFF-1];
udb_yrate.input = CurBuffer[yrateBUFF-1];
udb_zrate.input = CurBuffer[zrateBUFF-1];
udb_xaccel.input = CurBuffer[xaccelBUFF-1];
udb_yaccel.input = CurBuffer[yaccelBUFF-1];
udb_zaccel.input = CurBuffer[zaccelBUFF-1];
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].input = CurBuffer[analogInput1BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].input = CurBuffer[analogInput2BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].input = CurBuffer[analogInput3BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].input = CurBuffer[analogInput4BUFF-1];
#endif
#endif
DmaBuffer ^= 1; // Switch buffers
IFS0bits.DMA0IF = 0; // Clear the DMA0 Interrupt Flag
if (udb_flags._.a2d_read == 1) // prepare for the next reading
{
udb_flags._.a2d_read = 0;
udb_xrate.sum = udb_yrate.sum = udb_zrate.sum = 0;
udb_xaccel.sum = udb_yaccel.sum = udb_zaccel.sum = 0;
#ifdef VREF
udb_vref.sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].sum = 0;
#endif
sample_count = 0;
}
// perform the integration:
udb_xrate.sum += udb_xrate.input;
udb_yrate.sum += udb_yrate.input;
udb_zrate.sum += udb_zrate.input;
#ifdef VREF
udb_vref.sum += udb_vref.input;
#endif
udb_xaccel.sum += udb_xaccel.input;
udb_yaccel.sum += udb_yaccel.input;
udb_zaccel.sum += udb_zaccel.input;
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum += udb_analogInputs[0].input;
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].sum += udb_analogInputs[1].input;
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].sum += udb_analogInputs[2].input;
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].sum += udb_analogInputs[3].input;
#endif
sample_count ++;
// When there is a chance that read_gyros() and read_accel() will execute soon,
// have the new average values ready.
if (sample_count > ALMOST_ENOUGH_SAMPLES)
{
udb_xrate.value = __builtin_divsd(udb_xrate.sum , sample_count);
udb_yrate.value = __builtin_divsd(udb_yrate.sum , sample_count);
udb_zrate.value = __builtin_divsd(udb_zrate.sum , sample_count);
#ifdef VREF
udb_vref.value = __builtin_divsd(udb_vref.sum , sample_count);
#endif
udb_xaccel.value = __builtin_divsd(udb_xaccel.sum , sample_count);
udb_yaccel.value = __builtin_divsd(udb_yaccel.sum , sample_count);
udb_zaccel.value = __builtin_divsd(udb_zaccel.sum , sample_count);
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].value = __builtin_divsd(udb_analogInputs[0].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].value = __builtin_divsd(udb_analogInputs[1].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].value = __builtin_divsd(udb_analogInputs[2].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].value = __builtin_divsd(udb_analogInputs[3].sum, sample_count);
#endif
}
interrupt_restore_corcon;
}
void __attribute__((__interrupt__,__no_auto_psv__)) _T4Interrupt(void)
{
indicate_loading_inter;
// "set_ipl_on_output_pin" macro not here as _T4 Interrupt disabled when testing Interrupt Priorities
// interrupt_save_set_corcon;
switch (outputNum) {
case 0:
#if (USE_CASTLE_LINK_THROTTLE != 1)
HANDLE_SERVO_OUT(1, SERVO_OUT_PIN_1);
#else
cll_start_servo_out(SCALE_FOR_PWM_OUT(udb_pwOut[1]), SCALE_FOR_PWM_OUT(100));
outputNum++; // usually handled in HANDLE_SERVO_OUT()
#endif
break;
case 1:
#if (USE_CASTLE_LINK_THROTTLE != 1)
SERVO_OUT_PIN_1 = 0;
#else
cll_start_listening();
if (outputNum >= NUM_OUTPUTS) _T4IE = 0; // usually handled in HANDLE_SERVO_OUT()
#endif
HANDLE_SERVO_OUT(2, SERVO_OUT_PIN_2);
break;
case 2:
SERVO_OUT_PIN_2 = 0;
HANDLE_SERVO_OUT(3, SERVO_OUT_PIN_3);
break;
case 3:
SERVO_OUT_PIN_3 = 0;
HANDLE_SERVO_OUT(4, SERVO_OUT_PIN_4);
break;
case 4:
SERVO_OUT_PIN_4 = 0;
HANDLE_SERVO_OUT(5, SERVO_OUT_PIN_5);
break;
case 5:
SERVO_OUT_PIN_5 = 0;
HANDLE_SERVO_OUT(6, SERVO_OUT_PIN_6);
break;
case 6:
SERVO_OUT_PIN_6 = 0;
HANDLE_SERVO_OUT(7, SERVO_OUT_PIN_7);
break;
case 7:
SERVO_OUT_PIN_7 = 0;
HANDLE_SERVO_OUT(8, SERVO_OUT_PIN_8);
break;
case 8:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(9, SERVO_OUT_PIN_9);
break;
#ifdef SERVO_OUT_PIN_10
case 9:
SERVO_OUT_PIN_9 = 0;
HANDLE_SERVO_OUT(10, SERVO_OUT_PIN_10);
break;
case 10:
SERVO_OUT_PIN_10 = 0; // end the pulse by setting the SERVO_OUT_PIN_10 pin low
_T4IE = 0; // disable timer 4 interrupt
break;
#else
case 9:
SERVO_OUT_PIN_9 = 0;
_T4IE = 0; // disable timer 4 interrupt
break;
#endif // SERVO_OUT_PIN_10
}
_T4IF = 0; // clear the interrupt
#if (RECORD_FREE_STACK_SPACE == 1)
// Check stack space here because it's a high-priority ISR
// which may have interrupted a whole chain of other ISRs,
// So available stack space can get lowest here.
uint16_t stack = SP_current();
if (stack > maxstack)
{
maxstack = stack;
}
#endif
// interrupt_restore_corcon;
}
// When testing interrupt priorities, all servo output timings are sent to pin 8
// Pins 1-7 for are then indicating the timing of interrupt priorities
// No Servos should be attached to the board outputs, only a logic analyser.
void __attribute__((__interrupt__,__no_auto_psv__)) _T4Interrupt(void)
{
indicate_loading_inter;
set_ipl_on_output_pin;
// interrupt_save_set_corcon;
switch (outputNum) {
case 0:
HANDLE_SERVO_OUT(1, SERVO_OUT_PIN_8);
break;
case 1:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(2, SERVO_OUT_PIN_8);
break;
case 2:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(3, SERVO_OUT_PIN_8);
break;
case 3:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(4, SERVO_OUT_PIN_8);
break;
case 4:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(5, SERVO_OUT_PIN_8);
break;
case 5:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(6, SERVO_OUT_PIN_8);
break;
case 6:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(7, SERVO_OUT_PIN_8);
break;
case 7:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(8, SERVO_OUT_PIN_8);
break;
case 8:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(9, SERVO_OUT_PIN_8);
break;
#ifdef SERVO_OUT_PIN_10
case 9:
SERVO_OUT_PIN_8 = 0;
HANDLE_SERVO_OUT(10, SERVO_OUT_PIN_8);
break;
case 10:
SERVO_OUT_PIN_8 = 0; // end the pulse by setting the SERVO_OUT_PIN_10 pin low
_T4IE = 0; // disable timer 4 interrupt
break;
#else
case 9:
SERVO_OUT_PIN_9 = 0;
_T4IE = 0; // disable timer 4 interrupt
break;
#endif // SERVO_OUT_PIN_10
}
_T4IF = 0; // clear the interrupt
#if (RECORD_FREE_STACK_SPACE == 1)
// Check stack space here because it's a high-priority ISR
// which may have interrupted a whole chain of other ISRs,
// So available stack space can get lowest here.
uint16_t stack = SP_current();
if (stack > maxstack)
{
maxstack = stack;
}
#endif
// interrupt_restore_corcon;
unset_ipl_on_output_pin;
}
