From ee573a0275da67b82388b84805e09d5ef95beba6 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 28 Dec 2020 22:06:44 -0700 Subject: [PATCH] AP_Periph: add NTF (Notify) full library and it's params --- Tools/AP_Periph/AP_Periph.cpp | 69 ++++++++++++++++++++++------------ Tools/AP_Periph/AP_Periph.h | 31 ++++++++++++++- Tools/AP_Periph/Parameters.cpp | 10 ++++- Tools/AP_Periph/Parameters.h | 5 ++- Tools/AP_Periph/can.cpp | 60 ++++++++++++++++++----------- Tools/AP_Periph/rc_out.cpp | 12 +++--- Tools/AP_Periph/wscript | 2 + 7 files changed, 131 insertions(+), 58 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 31361a4a43..dda41f6753 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -126,12 +126,12 @@ void AP_Periph_FW::init() battery.lib.init(); #endif -#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_RC_OUT) +#if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_RC_OUT) hal.rcout->init(); #endif -#ifdef HAL_PERIPH_NEOPIXEL_COUNT - hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, AP_HAL::RCOutput::MODE_NEOPIXEL); +#ifdef HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY + hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, AP_HAL::RCOutput::MODE_NEOPIXEL); #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT @@ -173,24 +173,37 @@ void AP_Periph_FW::init() } #endif +#ifdef HAL_PERIPH_ENABLE_NOTIFY + notify.init(); +#endif + start_ms = AP_HAL::native_millis(); } -#if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8 +#if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) /* rotating rainbow pattern on startup */ -static void update_rainbow() +void AP_Periph_FW::update_rainbow() { +#ifdef HAL_PERIPH_ENABLE_NOTIFY + if (notify.get_led_len() != 8) { + return; + } +#endif static bool rainbow_done; if (rainbow_done) { return; } uint32_t now = AP_HAL::native_millis(); - if (now-start_ms > 1500) { + if (now - start_ms > 1500) { rainbow_done = true; - hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, -1, 0, 0, 0); - hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN); +#if defined (HAL_PERIPH_ENABLE_NOTIFY) + periph.notify.handle_rgb(0, 0, 0); +#elif defined(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY) + hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, -1, 0, 0, 0); + hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY); +#endif return; } static uint32_t last_update_ms; @@ -218,15 +231,22 @@ static void update_rainbow() float brightness = 0.3; for (uint8_t n=0; n<8; n++) { uint8_t i = (step + n) % nsteps; - hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, n, - rgb_rainbow[i].red*brightness, - rgb_rainbow[i].green*brightness, - rgb_rainbow[i].blue*brightness); +#if defined (HAL_PERIPH_ENABLE_NOTIFY) + periph.notify.handle_rgb( +#elif defined(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY) + hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, n, +#endif + rgb_rainbow[i].red*brightness, + rgb_rainbow[i].green*brightness, + rgb_rainbow[i].blue*brightness); } step++; - hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN); -} + +#if defined(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY) + hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY); #endif +} +#endif // HAL_PERIPH_ENABLE_NOTIFY #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE @@ -277,8 +297,8 @@ void AP_Periph_FW::update() #endif hal.scheduler->delay(1); #endif -#ifdef HAL_PERIPH_NEOPIXEL_COUNT - hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, HAL_PERIPH_NEOPIXEL_COUNT, AP_HAL::RCOutput::MODE_NEOPIXEL); +#ifdef HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY + hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY, AP_HAL::RCOutput::MODE_NEOPIXEL); #endif #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT @@ -314,9 +334,18 @@ void AP_Periph_FW::update() } #endif +#ifdef HAL_PERIPH_ENABLE_NOTIFY + static uint32_t notify_last_update_ms; + if (now - notify_last_update_ms >= 20) { + // update notify at 50Hz + notify_last_update_ms = now; + notify.update(); + } +#endif + can_update(); hal.scheduler->delay(1); -#if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8 +#if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) update_rainbow(); #endif #ifdef HAL_PERIPH_ENABLE_ADSB @@ -381,12 +410,6 @@ void AP_Periph_FW::check_for_serial_reboot_cmd(const int8_t serial_index) // This is copied from AP_Vehicle::reboot(bool hold_in_bootloader) minus the actual reboot void AP_Periph_FW::prepare_reboot() { -#ifdef HAL_PERIPH_ENABLE_NOTIFY - // Notify might want to blink some LEDs: - AP_Notify::flags.firmware_update = 1; - notify.update(); -#endif - #ifdef HAL_PERIPH_ENABLE_RC_OUT // force safety on hal.rcout->force_safety_on(); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index a6ca8e384d..04dce7c11b 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -6,6 +6,7 @@ #include #include #include "SRV_Channel/SRV_Channel.h" +#include #include #include #include @@ -14,10 +15,26 @@ #include "../AP_Bootloader/app_comms.h" #include "hwing_esc.h" -#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED) -#define AP_PERIPH_HAVE_LED +#if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY) +#define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #endif +#ifdef HAL_PERIPH_ENABLE_NOTIFY + #ifndef HAL_PERIPH_ENABLE_RC_OUT + #error "HAL_PERIPH_ENABLE_NOTIFY requires HAL_PERIPH_ENABLE_RC_OUT" + #endif + #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY + #error "You cannot enable HAL_PERIPH_ENABLE_NOTIFY and HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY at the same time. Notify already includes it" + #endif + #ifdef AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY + #error "You cannot enable HAL_PERIPH_ENABLE_NOTIFY and any HAL_PERIPH_ENABLE__LED_WITHOUT_NOTIFY at the same time. Notify already includes them all" + #endif + #ifdef HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY + #error "You cannot use HAL_PERIPH_ENABLE_NOTIFY and HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY at the same time. Notify already includes it. Set param OUTx_FUNCTION=120" + #endif +#endif + + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -133,6 +150,7 @@ public: #ifdef HAL_PERIPH_ENABLE_RC_OUT SRV_Channels servo_channels; + bool rcout_has_new_data_to_update; void rcout_init(); void rcout_init_1Hz(); @@ -142,6 +160,15 @@ public: void rcout_handle_safety_state(uint8_t safety_state); #endif + +#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) + void update_rainbow(); +#endif +#ifdef HAL_PERIPH_ENABLE_NOTIFY + // notification object for LEDs, buzzers etc + AP_Notify notify; +#endif + // setup the var_info table AP_Param param_loader{var_info}; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 8993591422..b3e7f1eae8 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -62,7 +62,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(serial_number, "BRD_SERIAL_NUM", 0), -#ifdef HAL_PERIPH_ENABLE_BUZZER +#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY GSCALAR(buzz_volume, "BUZZER_VOLUME", 100), #endif @@ -102,7 +102,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(baro_enable, "BARO_ENABLE", 1), #endif -#ifdef AP_PERIPH_HAVE_LED +#ifdef AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY GSCALAR(led_brightness, "LED_BRIGHTNESS", HAL_PERIPH_LED_BRIGHT_DEFAULT), #endif @@ -148,6 +148,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(msp_port, "MSP_PORT", AP_PERIPH_MSP_PORT_DEFAULT), #endif +#ifdef HAL_PERIPH_ENABLE_NOTIFY + // @Group: NTF_ + // @Path: ../libraries/AP_Notify/AP_Notify.cpp + GOBJECT(notify, "NTF_", AP_Notify), +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index b752b44e1b..53a8773391 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -36,15 +36,16 @@ public: k_param_rangefinder_port, k_param_gps_port, k_param_msp_port, + k_param_notify, }; AP_Int16 format_version; AP_Int16 can_node; AP_Int32 can_baudrate; -#ifdef HAL_PERIPH_ENABLE_BUZZER +#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY AP_Int8 buzz_volume; #endif -#ifdef AP_PERIPH_HAVE_LED +#ifdef AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY AP_Int8 led_brightness; #endif #ifdef HAL_PERIPH_ENABLE_BARO diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 033d97de28..e4ab225cdf 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -461,7 +461,7 @@ static void fix_float16(float &f) #pragma GCC diagnostic pop -#ifdef HAL_PERIPH_ENABLE_BUZZER +#if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) static uint32_t buzzer_start_ms; static uint32_t buzzer_len_ms; /* @@ -483,7 +483,11 @@ static void handle_beep_command(CanardInstance* ins, CanardRxTransfer* transfer) fix_float16(req.duration); buzzer_start_ms = AP_HAL::native_millis(); buzzer_len_ms = req.duration*1000; - float volume = constrain_float(periph.g.buzz_volume/100.0, 0, 1); +#ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY + float volume = constrain_float(periph.g.buzz_volume/100.0f, 0, 1); +#elif defined(HAL_PERIPH_ENABLE_NOTIFY) + float volume = constrain_float(periph.notify.get_buzz_volume()/100.0f, 0, 1); +#endif hal.util->toneAlarm_set_buzzer_tone(req.frequency, volume, uint32_t(req.duration*1000)); } @@ -500,7 +504,7 @@ static void can_buzzer_update(void) } } } -#endif // HAL_PERIPH_ENABLE_BUZZER +#endif // (HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || (HAL_PERIPH_ENABLE_NOTIFY) #if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT) static uint8_t safety_state; @@ -538,14 +542,20 @@ static void handle_RTCMStream(CanardInstance* ins, CanardRxTransfer* transfer) #endif // HAL_PERIPH_ENABLE_GPS -#ifdef AP_PERIPH_HAVE_LED +#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) { -#ifdef HAL_PERIPH_NEOPIXEL_COUNT - hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN, -1, red, green, blue); - hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN); -#endif // HAL_PERIPH_NEOPIXEL_COUNT -#ifdef HAL_PERIPH_ENABLE_NCP5623_LED +#ifdef HAL_PERIPH_ENABLE_NOTIFY + periph.notify.handle_rgb(red, green, blue); + periph.rcout_has_new_data_to_update = true; +#endif // HAL_PERIPH_ENABLE_NOTIFY + +#ifdef HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY + hal.rcout->set_serial_led_rgb_data(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY, -1, red, green, blue); + hal.rcout->serial_led_send(HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY); +#endif // HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY + +#ifdef HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY { const uint8_t i2c_address = 0x38; static AP_HAL::OwnPtr dev; @@ -563,8 +573,9 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) v = 0x80 | blue >> 3; // blue dev->transfer(&v, 1, nullptr, 0); } -#endif // HAL_PERIPH_ENABLE_NCP5623_LED -#ifdef HAL_PERIPH_ENABLE_NCP5623_BGR_LED +#endif // HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY + +#ifdef HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY { const uint8_t i2c_address = 0x38; static AP_HAL::OwnPtr dev; @@ -582,8 +593,8 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) v = 0x80 | red >> 3; // red dev->transfer(&v, 1, nullptr, 0); } -#endif // HAL_PERIPH_ENABLE_NCP5623_BGR_LED -#ifdef HAL_PERIPH_ENABLE_TOSHIBA_LED +#endif // HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY +#ifdef HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY { #define TOSHIBA_LED_PWM0 0x01 // pwm0 register #define TOSHIBA_LED_ENABLE 0x04 // enable register @@ -608,7 +619,7 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue) }; dev_toshiba->transfer(val, sizeof(val), nullptr, 0); } -#endif // HAL_PERIPH_ENABLE_TOSHIBA_LED +#endif // HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY } /* @@ -629,8 +640,13 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer uint8_t red = cmd.color.red<<3; uint8_t green = (cmd.color.green>>1)<<3; uint8_t blue = cmd.color.blue<<3; - if (periph.g.led_brightness != 100 && periph.g.led_brightness >= 0) { - float scale = periph.g.led_brightness * 0.01; +#ifdef HAL_PERIPH_ENABLE_NOTIFY + const int8_t brightness = periph.notify.get_rgb_led_brightness_percent(); +#elif defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) + const int8_t brightness = periph.g.led_brightness; +#endif + if (brightness != 100 && brightness >= 0) { + const float scale = brightness * 0.01; red = constrain_int16(red * scale, 0, 255); green = constrain_int16(green * scale, 0, 255); blue = constrain_int16(blue * scale, 0, 255); @@ -638,7 +654,7 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer set_rgb_led(red, green, blue); } } -#endif // AP_PERIPH_HAVE_LED +#endif // AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY #ifdef HAL_PERIPH_ENABLE_RC_OUT static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfer) @@ -815,7 +831,7 @@ static void onTransferReceived(CanardInstance* ins, handle_param_executeopcode(ins, transfer); break; -#ifdef HAL_PERIPH_ENABLE_BUZZER +#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID: handle_beep_command(ins, transfer); break; @@ -833,7 +849,7 @@ static void onTransferReceived(CanardInstance* ins, break; #endif -#ifdef AP_PERIPH_HAVE_LED +#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID: handle_lightscommand(ins, transfer); break; @@ -897,7 +913,7 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; return true; -#ifdef HAL_PERIPH_ENABLE_BUZZER +#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE; return true; @@ -907,7 +923,7 @@ static bool shouldAcceptTransfer(const CanardInstance* ins, *out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE; return true; #endif -#ifdef AP_PERIPH_HAVE_LED +#if defined(AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NOTIFY) case UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_SIGNATURE; return true; @@ -1322,7 +1338,7 @@ void AP_Periph_FW::can_update() can_baro_update(); can_airspeed_update(); can_rangefinder_update(); -#ifdef HAL_PERIPH_ENABLE_BUZZER +#if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY) can_buzzer_update(); #endif #ifdef HAL_GPIO_PIN_SAFE_LED diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 17d2e22305..fdc3a125b9 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -28,8 +28,6 @@ #error "You must define a PWM output in your hwdef.dat" #endif -static bool has_new_data_to_update; - extern const AP_HAL::HAL &hal; void AP_Periph_FW::rcout_init() @@ -73,7 +71,7 @@ void AP_Periph_FW::rcout_esc(int16_t *rc, uint8_t num_channels) SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), rc[i]); } - has_new_data_to_update = true; + rcout_has_new_data_to_update = true; } void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value) @@ -86,7 +84,7 @@ void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value) const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1); SRV_Channels::set_output_norm(function, command_value); - has_new_data_to_update = true; + rcout_has_new_data_to_update = true; } void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state) @@ -96,15 +94,15 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state) } else { hal.rcout->force_safety_on(); } - has_new_data_to_update = true; + rcout_has_new_data_to_update = true; } void AP_Periph_FW::rcout_update() { - if (!has_new_data_to_update) { + if (!rcout_has_new_data_to_update) { return; } - has_new_data_to_update = false; + rcout_has_new_data_to_update = false; SRV_Channels::calc_pwm(); SRV_Channels::cork(); diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 905fe40acb..660e909e3e 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -38,6 +38,8 @@ def build(bld): 'AP_ROMFS', 'AP_MSP', 'SRV_Channel', + 'AP_Notify', + 'AP_SerialLED', ], exclude_src=[ 'libraries/AP_HAL_ChibiOS/Storage.cpp'