Plane: updates for new notify API
This commit is contained in:
parent
7b1e2b0e78
commit
a93f7c39a9
@ -71,9 +71,6 @@
|
||||
#include <AP_TECS.h>
|
||||
|
||||
#include <AP_Notify.h> // Notify library
|
||||
#include <AP_BoardLED.h> // BoardLEDs library
|
||||
#include <ToshibaLED.h> // ToshibaLED library
|
||||
#include <ToshibaLED_PX4.h> // ToshibaLED library for PX4
|
||||
|
||||
// Pre-AP_HAL compatibility
|
||||
#include "compat.h"
|
||||
@ -132,6 +129,9 @@ static RC_Channel *channel_pitch;
|
||||
static RC_Channel *channel_throttle;
|
||||
static RC_Channel *channel_rudder;
|
||||
|
||||
// notification object for LEDs, buzzers etc
|
||||
static AP_Notify notify;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// prototypes
|
||||
static void update_events(void);
|
||||
@ -386,16 +386,6 @@ static struct {
|
||||
} failsafe;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// LED output
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static AP_BoardLED board_led;
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
static ToshibaLED_PX4 toshiba_led;
|
||||
#else
|
||||
static ToshibaLED toshiba_led;
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// GPS variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -722,7 +712,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||
{ read_battery, 5, 1000 },
|
||||
{ compass_accumulate, 1, 1500 },
|
||||
{ barometer_accumulate, 1, 900 }, // 20
|
||||
{ update_toshiba_led, 1, 100 },
|
||||
{ update_notify, 1, 100 },
|
||||
{ one_second_loop, 50, 3900 },
|
||||
{ check_long_failsafe, 15, 1000 },
|
||||
{ airspeed_ratio_update, 50, 1000 },
|
||||
@ -744,15 +734,10 @@ void setup() {
|
||||
AP_Param::setup_sketch_defaults();
|
||||
|
||||
// arduplane does not use arming nor pre-arm checks
|
||||
notify.init();
|
||||
AP_Notify::flags.armed = true;
|
||||
AP_Notify::flags.pre_arm_check = true;
|
||||
|
||||
// initialise board leds
|
||||
board_led.init();
|
||||
|
||||
// initialise toshiba led
|
||||
toshiba_led.init();
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
|
||||
|
||||
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
|
||||
|
@ -2144,6 +2144,7 @@ static void mavlink_delay_cb()
|
||||
last_50hz = tnow;
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
notify.update();
|
||||
}
|
||||
if (tnow - last_5s > 5000) {
|
||||
last_5s = tnow;
|
||||
|
@ -483,11 +483,11 @@ static void startup_INS_ground(bool do_accel_init)
|
||||
digitalWrite(C_LED_PIN, LED_OFF);
|
||||
}
|
||||
|
||||
// update_toshiba_led - updates the status of the toshiba led
|
||||
// updates the status of the notify objects
|
||||
// should be called at 50hz
|
||||
static void update_toshiba_led()
|
||||
static void update_notify()
|
||||
{
|
||||
AP_Notify::update();
|
||||
notify.update();
|
||||
}
|
||||
|
||||
static void resetPerfData(void) {
|
||||
|
Loading…
Reference in New Issue
Block a user