AP_Periph: add NTF (Notify) full library and it's params

This commit is contained in:
Tom Pittenger 2020-12-28 22:06:44 -07:00 committed by Tom Pittenger
parent 113f792ad1
commit ee573a0275
7 changed files with 131 additions and 58 deletions

View File

@ -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();

View File

@ -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};

View File

@ -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
};

View File

@ -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

View File

@ -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

View File

@ -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();

View File

@ -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'