Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Scripting: RGB LED override binding #12892

Open
wants to merge 5 commits into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -352,6 +352,16 @@ void AP_Notify::handle_led_control(const mavlink_message_t &msg)
}
}

// handle RGB override
void AP_Notify::handle_rgb_override(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz, uint16_t duration_ms)
{
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->rgb_override(r, g, b, rate_hz, duration_ms);
}
}
}

// handle a PLAY_TUNE message
void AP_Notify::handle_play_tune(const mavlink_message_t &msg)
{
@@ -138,6 +138,9 @@ class AP_Notify
// handle a LED_CONTROL message
static void handle_led_control(const mavlink_message_t &msg);

// handle RGB override
static void handle_rgb_override(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz, uint16_t duration_ms);

// handle a PLAY_TUNE message
static void handle_play_tune(const mavlink_message_t &msg);

@@ -23,6 +23,11 @@ class NotifyDevice {
// play a MML tune
virtual void play_tune(const char *tune) {}

// RGB override
// override other all methods for given duration in ms (0 = for ever)
// used with scripting
virtual void rgb_override(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz, uint16_t duration_ms) {}

// this pointer is used to read the parameters relative to devices
const AP_Notify *pNotify;
};
@@ -54,13 +54,18 @@ void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)

RGBLed::rgb_source_t RGBLed::rgb_source() const
{
if (_led_override_duration.active) {
return rgb_source_t::duration_override;
}

return rgb_source_t(pNotify->_rgb_led_override.get());
}

// set_rgb - set color as a combination of red, green and blue values
void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
if (rgb_source() == mavlink) {
const rgb_source_t source = rgb_source();
if (source == mavlink || source == duration_override) {
// don't set if in override mode
return;
}
@@ -190,12 +195,21 @@ uint32_t RGBLed::get_colour_sequence_traffic_light(void) const
// at 50Hz
void RGBLed::update()
{
// check if duration override has timed out
const uint32_t now = AP_HAL::millis();
if ((now - _led_override_duration.led_override.start_ms) > _led_override_duration.duration_ms && _led_override_duration.duration_ms != 0) {
_led_override_duration.active = false;
}

uint32_t current_colour_sequence = 0;

switch (rgb_source()) {
case mavlink:
update_override();
update_override(_led_override);
return; // note this is a return not a break!
case duration_override:
update_override(_led_override_duration.led_override);
return; // this too!
case standard:
current_colour_sequence = get_colour_sequence();
break;
@@ -209,7 +223,7 @@ void RGBLed::update()

const uint8_t brightness = get_brightness();

uint8_t step = (AP_HAL::millis()/100) % 10;
uint8_t step = (now/100) % 10;

// ensure we can't skip a step even with awful timing
if (step != last_step) {
@@ -264,20 +278,36 @@ void RGBLed::handle_led_control(const mavlink_message_t &msg)
/*
update LED when in override mode
*/
void RGBLed::update_override(void)
void RGBLed::update_override(override override_val)
{
if (_led_override.rate_hz == 0) {
if (override_val.rate_hz == 0) {
// solid colour
_set_rgb(_led_override.r, _led_override.g, _led_override.b);
_set_rgb(override_val.r, override_val.g, override_val.b);
return;
}
// blinking
uint32_t ms_per_cycle = 1000 / _led_override.rate_hz;
uint32_t cycle = (AP_HAL::millis() - _led_override.start_ms) % ms_per_cycle;
uint32_t ms_per_cycle = 1000 / override_val.rate_hz;
uint32_t cycle = (AP_HAL::millis() - override_val.start_ms) % ms_per_cycle;
if (cycle > ms_per_cycle / 2) {
// on
_set_rgb(_led_override.r, _led_override.g, _led_override.b);
_set_rgb(override_val.r, override_val.g, override_val.b);
} else {
_set_rgb(0, 0, 0);
}
}

/*
RGB override
override other all methods for given duration in ms (0 = for ever)
used with scripting
*/
void RGBLed::rgb_override(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz, uint16_t duration_ms)
{
_led_override_duration.led_override.r = r;
_led_override_duration.led_override.g = g;
_led_override_duration.led_override.b = b;
_led_override_duration.led_override.rate_hz = rate_hz;
_led_override_duration.led_override.start_ms = AP_HAL::millis();
_led_override_duration.duration_ms = duration_ms;
_led_override_duration.active = true;
}
@@ -38,7 +38,12 @@ class RGBLed: public NotifyDevice {

// handle LED control, only used when LED_OVERRIDE=1
virtual void handle_led_control(const mavlink_message_t &msg) override;


// RGB override
// override other all methods for given duration in ms (0 = for ever)
// used with scripting
virtual void rgb_override(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz, uint16_t duration_ms) override;

protected:
// methods implemented in hardware specific classes
virtual bool hw_init(void) = 0;
@@ -47,8 +52,6 @@ class RGBLed: public NotifyDevice {
// set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15
virtual void _set_rgb(uint8_t red, uint8_t green, uint8_t blue);

void update_override();

// meta-data common to all hw devices
uint8_t _red_des, _green_des, _blue_des; // color requested by timed update
uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led
@@ -57,12 +60,21 @@ class RGBLed: public NotifyDevice {
uint8_t _led_medium;
uint8_t _led_dim;

struct {
struct override {

This comment has been minimized.

Copy link
@peterbarker

peterbarker Dec 3, 2019

Contributor

Should we really be using a keyword here? :-)

uint8_t r, g, b;
uint8_t rate_hz;
uint32_t start_ms;
} _led_override;

};
override _led_override;

struct {
override led_override;
uint16_t duration_ms;
bool active;
} _led_override_duration;

void update_override(override override_val);

private:
void update_colours();
uint32_t get_colour_sequence() const;
@@ -110,6 +122,7 @@ class RGBLed: public NotifyDevice {
mavlink = 1,
obc = 2,
traffic_light = 3,
duration_override = 4,
};
rgb_source_t rgb_source() const;

@@ -0,0 +1,38 @@
-- This script is a test of led override

local count = 0

function update() -- this is the loop which periodically runs

count = count + 1

if count == 1 then
-- solid red for 15 seconds
-- red,green,blue,rate hz, duration ms
notify:handle_rgb_override(255,0,0,0,15000)
elseif count == 2 then
-- solid green for 15 seconds
notify:handle_rgb_override(0,255,0,0,15000)
elseif count == 3 then
-- solid blue for 15 seconds
notify:handle_rgb_override(0,0,255,0,15000)
elseif count == 4 then
-- 1hz red + green for 15 seconds
notify:handle_rgb_override(255,255,0,1,15000)
elseif count == 5 then
-- 1hz green + blue for 15 seconds
notify:handle_rgb_override(0,255,255,1,15000)
elseif count == 6 then
-- 1hz red + blue for 15 seconds
notify:handle_rgb_override(255,0,255,1,15000)
elseif count == 7 then
-- fast white for 15 seconds
notify:handle_rgb_override(255,255,255,10,15000)
count = 0
end

return update, 30000 -- reschedules the loop in 30 seconds

end

return update() -- run immediately before starting to reschedule
@@ -108,6 +108,7 @@ userdata Vector2f operator -
include AP_Notify/AP_Notify.h
singleton AP_Notify alias notify
singleton AP_Notify method play_tune void string
singleton AP_Notify method handle_rgb_override void uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX uint16_t 0 UINT16_MAX

include AP_RangeFinder/AP_RangeFinder.h

@@ -832,6 +832,38 @@ static int RangeFinder_num_sensors(lua_State *L) {
return 1;
}

static int AP_Notify_handle_rgb_override(lua_State *L) {
AP_Notify * ud = AP_Notify::get_singleton();
if (ud == nullptr) {
return luaL_argerror(L, 1, "notify not supported on this firmware");
}

binding_argcheck(L, 6);
const lua_Integer raw_data_2 = luaL_checkinteger(L, 2);
luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT8_MAX, UINT8_MAX))), 2, "argument out of range");
const uint8_t data_2 = static_cast<uint8_t>(raw_data_2);
const lua_Integer raw_data_3 = luaL_checkinteger(L, 3);
luaL_argcheck(L, ((raw_data_3 >= MAX(0, 0)) && (raw_data_3 <= MIN(UINT8_MAX, UINT8_MAX))), 3, "argument out of range");
const uint8_t data_3 = static_cast<uint8_t>(raw_data_3);
const lua_Integer raw_data_4 = luaL_checkinteger(L, 4);
luaL_argcheck(L, ((raw_data_4 >= MAX(0, 0)) && (raw_data_4 <= MIN(UINT8_MAX, UINT8_MAX))), 4, "argument out of range");
const uint8_t data_4 = static_cast<uint8_t>(raw_data_4);
const lua_Integer raw_data_5 = luaL_checkinteger(L, 5);
luaL_argcheck(L, ((raw_data_5 >= MAX(0, 0)) && (raw_data_5 <= MIN(UINT8_MAX, UINT8_MAX))), 5, "argument out of range");
const uint8_t data_5 = static_cast<uint8_t>(raw_data_5);
const lua_Integer raw_data_6 = luaL_checkinteger(L, 6);
luaL_argcheck(L, ((raw_data_6 >= MAX(0, 0)) && (raw_data_6 <= MIN(UINT16_MAX, UINT16_MAX))), 6, "argument out of range");
const uint16_t data_6 = static_cast<uint16_t>(raw_data_6);
ud->handle_rgb_override(
data_2,
data_3,
data_4,
data_5,
data_6);

return 0;
}

static int AP_Notify_play_tune(lua_State *L) {
AP_Notify * ud = AP_Notify::get_singleton();
if (ud == nullptr) {
@@ -1735,6 +1767,7 @@ const luaL_Reg RangeFinder_meta[] = {
};

const luaL_Reg AP_Notify_meta[] = {
{"handle_rgb_override", AP_Notify_handle_rgb_override},
{"play_tune", AP_Notify_play_tune},
{NULL, NULL}
};
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.