diff --git a/libraries/Desktop/support/Arduino.cpp b/libraries/Desktop/support/Arduino.cpp index 34db6f58be9..bbcb0c9918d 100644 --- a/libraries/Desktop/support/Arduino.cpp +++ b/libraries/Desktop/support/Arduino.cpp @@ -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(); + } +} diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index 8f7c75a2d51..7b241a9a216 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -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)); diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h index 7a028f16df0..3cfc729292d 100644 --- a/libraries/Desktop/support/util.h +++ b/libraries/Desktop/support/util.h @@ -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);