Skip to content

Commit

Permalink
AP_Notify: fixed LED sync for OreoLED
Browse files Browse the repository at this point in the history 
the NuttX driver tries to send a sync every 4s, but actually ends up
sending at around 4.1s due to poor scheduling. Rather strangely, the
oreoled firmware seems to rely on this inaccuracy, and doesn't work
with exactly 4s under ChibiOS
  • Loading branch information
@tridge
tridge committed Nov 26, 2018
1 parent 78dc575 commit 891447d
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 63 deletions.
88 changes: 30 additions & 58 deletions libraries/AP_Notify/OreoLED_I2C.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,6 @@ void OreoLED_I2C::update()
return; // slow rate from 50hz to 10hz
}

sync_counter(); // syncronizes LEDs every 10 seconds

if (mode_firmware_update()) {
return; // don't go any further if the Pixhawk is in firmware update
}
Expand Down Expand Up @@ -122,18 +120,6 @@ bool OreoLED_I2C::slow_counter()
}


// Calls resyncing the LEDs every 10 seconds
// Always returns false, no action needed.
void OreoLED_I2C::sync_counter()
{
_sync_count++;
if (_sync_count > 100) {
_sync_count = 0;
send_sync();
}
}


// Procedure for when Pixhawk is in FW update / bootloader
// Makes all LEDs go into color cycle mode
// Returns true if firmware update in progress. False if not
Expand Down Expand Up @@ -389,21 +375,6 @@ void OreoLED_I2C::set_macro(uint8_t instance, oreoled_macro macro)
}
}

// send_sync - force a syncronisation of the all LED's
void OreoLED_I2C::send_sync()
{
WITH_SEMAPHORE(_sem);

// store desired macro for all LEDs
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {
_state_desired[i].send_sync();
if (!(_state_desired[i] == _state_sent[i])) {
_send_required = true;
}
}
}


// Clear the desired state
void OreoLED_I2C::clear_state(void)
{
Expand Down Expand Up @@ -457,11 +428,6 @@ void OreoLED_I2C::boot_leds(void)
// update_timer - called by scheduler and updates PX4 driver with commands
void OreoLED_I2C::update_timer(void)
{
// exit immediately if send not required, or state is being updated
if (!_send_required) {
return;
}

WITH_SEMAPHORE(_sem);

uint32_t now = AP_HAL::millis();
Expand All @@ -474,6 +440,21 @@ void OreoLED_I2C::update_timer(void)
boot_leds();
}

// send a sync every 4.1s. The PX4 driver uses 4ms, but using
// exactly 4ms does not work. It seems that the oreoled firmware
// relies on the inaccuracy of the NuttX scheduling for this to
// work, and exactly 4ms from ChibiOS causes syncronisation to
// fail
if (now - _last_sync_ms > 4100) {
_last_sync_ms = now;
send_sync();
}

// exit immediately if send not required, or state is being updated
if (!_send_required) {
return;
}

// for each LED
for (uint8_t i=0; i<OREOLED_NUM_LEDS; i++) {

Expand Down Expand Up @@ -535,18 +516,6 @@ void OreoLED_I2C::update_timer(void)
break;
}

case OREOLED_MODE_SYNC: {
/* set I2C address to zero */
_dev->set_address(0);

/* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
uint8_t msg[] = {0x01, 0x00};

/* send I2C command */
_dev->transfer(msg, sizeof(msg), nullptr, 0);
break;
}

default:
break;
};
Expand All @@ -559,6 +528,21 @@ void OreoLED_I2C::update_timer(void)
_send_required = false;
}

void OreoLED_I2C::send_sync(void)
{
/* set I2C address to zero */
_dev->set_address(0);

/* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
uint8_t msg[] = {0x01, 0x00};

/* send I2C command */
_dev->set_retries(0);
_dev->transfer(msg, sizeof(msg), nullptr, 0);
_dev->set_retries(2);
}



// Handle an LED_CONTROL mavlink message
void OreoLED_I2C::handle_led_control(mavlink_message_t *msg)
Expand Down Expand Up @@ -635,12 +619,6 @@ void OreoLED_I2C::handle_led_control(mavlink_message_t *msg)
set_rgb(packet.instance, pattern, packet.custom_bytes[CUSTOM_HEADER_LENGTH + 1], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 2],
packet.custom_bytes[CUSTOM_HEADER_LENGTH + 3], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 4], packet.custom_bytes[CUSTOM_HEADER_LENGTH + 5],
packet.custom_bytes[CUSTOM_HEADER_LENGTH + 6], period, phase_offset);
} else if (memcmp(packet.custom_bytes, "SYNC", CUSTOM_HEADER_LENGTH) == 0) { // check for the SYNC sub-command
// check to make sure the total length matches the length of the SYN0 command + data values
if (packet.custom_len != CUSTOM_HEADER_LENGTH + 0) {
return;
}
send_sync();
} else { // unrecognized command
return;
}
Expand Down Expand Up @@ -672,12 +650,6 @@ void OreoLED_I2C::oreo_state::clear_state()
phase_offset = 0;
}

void OreoLED_I2C::oreo_state::send_sync()
{
clear_state();
mode = OREOLED_MODE_SYNC;
}

void OreoLED_I2C::oreo_state::set_macro(oreoled_macro new_macro)
{
clear_state();
Expand Down
6 changes: 1 addition & 5 deletions libraries/AP_Notify/OreoLED_I2C.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,6 @@ class OreoLED_I2C : public NotifyDevice {

// functions to set LEDs to specific patterns. These functions return true if no further updates should be made to LEDs this iteration
bool slow_counter(void);
void sync_counter(void);
bool mode_firmware_update(void);
bool mode_init(void);
bool mode_failsafe_radio(void);
Expand All @@ -128,7 +127,6 @@ class OreoLED_I2C : public NotifyDevice {
OREOLED_MODE_MACRO,
OREOLED_MODE_RGB,
OREOLED_MODE_RGB_EXTENDED,
OREOLED_MODE_SYNC
};

// Oreo LED modes
Expand Down Expand Up @@ -157,8 +155,6 @@ class OreoLED_I2C : public NotifyDevice {

void clear_state();

void send_sync();

void set_macro(oreoled_macro new_macro);

void set_rgb(enum oreoled_pattern new_pattern, uint8_t new_red, uint8_t new_green, uint8_t new_blue);
Expand Down Expand Up @@ -195,8 +191,8 @@ class OreoLED_I2C : public NotifyDevice {
uint8_t _rear_color_b = 255; // the rear LED blue value

uint8_t _slow_count;
uint8_t _sync_count = 80;
uint8_t _boot_count;
uint32_t _last_boot_ms;
uint32_t _last_sync_ms;
};

0 comments on commit 891447d

Please sign in to comment.