AP_Periph: add NTF (Notify) full library and it's params
This commit is contained in:
parent
113f792ad1
commit
ee573a0275
@ -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();
|
||||
|
@ -6,6 +6,7 @@
|
||||
#include <AP_Compass/AP_Compass.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include "SRV_Channel/SRV_Channel.h"
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <AP_Airspeed/AP_Airspeed.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
@ -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_<device>_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};
|
||||
|
||||
|
@ -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
|
||||
};
|
||||
|
||||
|
@ -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
|
||||
|
@ -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<AP_HAL::I2CDevice> 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<AP_HAL::I2CDevice> 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
|
||||
|
@ -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();
|
||||
|
@ -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'
|
||||
|
Loading…
Reference in New Issue
Block a user