mirror of https://github.com/ArduPilot/ardupilot
64 lines
1.1 KiB
Plaintext
64 lines
1.1 KiB
Plaintext
|
/*
|
||
|
|
test interrupt masking methods on APM1
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <FastSerial.h>
|
||
|
|
#include <AP_Common.h>
|
||
|
|
#include <AP_Math.h>
|
||
|
|
#include <Arduino_Mega_ISR_Registry.h>
|
||
|
|
#include <APM_RC.h>
|
||
|
|
|
||
|
|
FastSerialPort0(Serial);
|
||
|
|
|
||
|
|
Arduino_Mega_ISR_Registry isr_registry;
|
||
|
|
APM_RC_APM1 APM_RC;
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
Serial.begin(115200);
|
||
|
|
isr_registry.init();
|
||
|
|
APM_RC.Init(&isr_registry);
|
||
|
|
Serial.println("Interrupt masking test");
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
static uint16_t last_values[8];
|
||
|
|
|
||
|
|
#define USE_CLI 0
|
||
|
|
#define USE_MASK 1
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
if (APM_RC.GetState() == 1) {
|
||
|
|
bool changed = false;
|
||
|
|
for (uint8_t i = 0; i < 8; i++) {
|
||
|
|
uint16_t v = APM_RC.InputCh(i);
|
||
|
|
if (abs(v - last_values[i]) > 10) {
|
||
|
|
changed = true;
|
||
|
|
}
|
||
|
|
last_values[i] = v;
|
||
|
|
}
|
||
|
|
if (changed) {
|
||
|
|
for (uint8_t i = 0; i < 8; i++) {
|
||
|
|
Serial.printf("%u:%4u ", (unsigned)i, (unsigned)last_values[i]);
|
||
|
|
}
|
||
|
|
Serial.println();
|
||
|
|
delayMicroseconds(500);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#if USE_CLI
|
||
|
|
uint8_t oldSREG = SREG;
|
||
|
|
cli();
|
||
|
|
delayMicroseconds(100);
|
||
|
|
SREG = oldSREG;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#if USE_MASK
|
||
|
|
uint8_t _timsk4 = TIMSK4;
|
||
|
|
TIMSK4 &= ~(1<<ICIE4);
|
||
|
|
delayMicroseconds(100);
|
||
|
|
TIMSK4 = _timsk4;
|
||
|
|
#endif
|
||
|
|
}
|