Browse Source

desktop: added attachInterrupt() support

mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
9bb0bd0341
  1. 25
      libraries/Desktop/support/Arduino.cpp
  2. 1
      libraries/Desktop/support/sitl_adc.cpp
  3. 1
      libraries/Desktop/support/util.h

25
libraries/Desktop/support/Arduino.cpp

@ -156,3 +156,28 @@ char *ltoa(long __val, char *__s, int __radix) @@ -156,3 +156,28 @@ char *ltoa(long __val, char *__s, int __radix)
return __s;
}
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
static struct {
void (*call)(void);
} interrupt_table[7];
void attachInterrupt(uint8_t inum, void (*call)(void), int mode)
{
if (inum >= ARRAY_LENGTH(interrupt_table)) {
fprintf(stderr, "Bad attachInterrupt to interrupt %u\n", inum);
exit(1);
}
interrupt_table[inum].call = call;
}
void runInterrupt(uint8_t inum)
{
if (inum >= ARRAY_LENGTH(interrupt_table)) {
fprintf(stderr, "Bad runInterrupt to interrupt %u\n", inum);
exit(1);
}
if (interrupt_table[inum].call != NULL) {
interrupt_table[inum].call();
}
}

1
libraries/Desktop/support/sitl_adc.cpp

@ -86,6 +86,7 @@ void sitl_update_adc(float roll, float pitch, float yaw, @@ -86,6 +86,7 @@ void sitl_update_adc(float roll, float pitch, float yaw,
UDR2.set(sensor_map[i], adc[i]);
}
runInterrupt(6);
// set the airspeed sensor input
UDR2.set(7, airspeed_sensor(airspeed));

1
libraries/Desktop/support/util.h

@ -6,3 +6,4 @@ @@ -6,3 +6,4 @@
void set_nonblocking(int fd);
double normalise(double v, double min, double max);
double normalise180(double v);
void runInterrupt(uint8_t inum);

Loading…
Cancel
Save