Staroselskii Georgii
10 years ago
committed by
Andrew Tridgell
22 changed files with 696 additions and 383 deletions
@ -0,0 +1,37 @@
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* AP_Notify Library.
|
||||
* based upon a prototype library by David "Buzz" Bussenschutt. |
||||
*/ |
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#ifndef __LED_H__ |
||||
#define __LED_H__ |
||||
|
||||
#include <AP_HAL.h> |
||||
#include "NotifyDevice.h" |
||||
|
||||
class Led: public NotifyDevice { |
||||
public: |
||||
virtual ~Led() {} |
||||
// init - initialised the LED
|
||||
virtual bool init(void) = 0; |
||||
// update - updates led according to timed_updated. Should be
|
||||
// called at 50Hz
|
||||
virtual void update() = 0; |
||||
}; |
||||
|
||||
#endif //__LED_H__
|
||||
@ -0,0 +1,29 @@
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
NavioLED driver |
||||
*/ |
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
*/ |
||||
|
||||
#include "NavioLED.h" |
||||
|
||||
#define NAVIO_LED_BRIGHT 0x0 // full brightness
|
||||
#define NAVIO_LED_MEDIUM 0x7F // medium brightness
|
||||
#define NAVIO_LED_DIM 0x4F // dim brightness
|
||||
#define NAVIO_LED_OFF 0xFF // off
|
||||
|
||||
NavioLED::NavioLED() :
|
||||
RGBLed(NAVIO_LED_OFF, NAVIO_LED_BRIGHT, NAVIO_LED_MEDIUM, NAVIO_LED_DIM) |
||||
{ |
||||
|
||||
} |
||||
@ -0,0 +1,31 @@
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* AP_Notify Library.
|
||||
* based upon a prototype library by David "Buzz" Bussenschutt. |
||||
*/ |
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#ifndef __NAVIO_LED_H__ |
||||
#define __NAVIO_LED_H__ |
||||
|
||||
#include "RGBLed.h" |
||||
|
||||
class NavioLED: public RGBLed { |
||||
public: |
||||
NavioLED(); |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,88 @@
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
NavioLED I2C driver |
||||
*/ |
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#include <AP_HAL.h> |
||||
#include "NavioLED_I2C.h" |
||||
|
||||
#define PCA9685_ADDRESS 0x40 |
||||
#define PCA9685_PWM 0x6 |
||||
|
||||
extern const AP_HAL::HAL& hal; |
||||
|
||||
bool NavioLED_I2C::hw_init() |
||||
{ |
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); |
||||
|
||||
// take i2c bus sempahore
|
||||
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { |
||||
return false; |
||||
} |
||||
|
||||
// disable recording of i2c lockup errors
|
||||
hal.i2c->ignore_errors(true); |
||||
|
||||
// enable the led
|
||||
bool ret = true; |
||||
|
||||
// re-enable recording of i2c lockup errors
|
||||
hal.i2c->ignore_errors(false); |
||||
|
||||
// give back i2c semaphore
|
||||
i2c_sem->give(); |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
// set_rgb - set color as a combination of red, green and blue values
|
||||
bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) |
||||
{ |
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); |
||||
|
||||
// exit immediately if we can't take the semaphore
|
||||
if (i2c_sem == NULL || !i2c_sem->take(5)) { |
||||
return false; |
||||
} |
||||
|
||||
uint16_t red_adjusted = red * 0x10; |
||||
uint16_t green_adjusted = green * 0x10; |
||||
uint16_t blue_adjusted = blue * 0x10; |
||||
|
||||
uint8_t blue_channel_lsb = blue_adjusted & 0xFF; |
||||
uint8_t blue_channel_msb = blue_adjusted >> 8; |
||||
|
||||
uint8_t green_channel_lsb = green_adjusted & 0xFF; |
||||
uint8_t green_channel_msb = green_adjusted >> 8; |
||||
|
||||
uint8_t red_channel_lsb = red_adjusted & 0xFF; |
||||
uint8_t red_channel_msb = red_adjusted >> 8; |
||||
|
||||
|
||||
uint8_t transaction[] = {0x00, 0x00, blue_channel_lsb, blue_channel_msb, |
||||
0x00, 0x00, green_channel_lsb, green_channel_msb, |
||||
0x00, 0x00, red_channel_lsb, red_channel_msb |
||||
}; |
||||
|
||||
|
||||
bool success = (hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_PWM, sizeof(transaction), transaction) == 0); |
||||
|
||||
// give back i2c semaphore
|
||||
i2c_sem->give(); |
||||
return success; |
||||
} |
||||
@ -0,0 +1,29 @@
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
NavioLED I2C driver |
||||
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#ifndef __NAVIO_LED_I2C_H__ |
||||
#define __NAVIO_LED_I2C_H__ |
||||
|
||||
#include "NavioLED.h" |
||||
|
||||
class NavioLED_I2C : public NavioLED |
||||
{ |
||||
protected: |
||||
virtual bool hw_init(void); |
||||
virtual bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b); |
||||
}; |
||||
|
||||
#endif // __TOSHIBA_LED_I2C_H__
|
||||
@ -0,0 +1,14 @@
@@ -0,0 +1,14 @@
|
||||
#ifndef __NOTIFYDEVICE_H__ |
||||
#define __NOTIFYDEVICE_H__ |
||||
|
||||
class NotifyDevice { |
||||
public: |
||||
virtual ~NotifyDevice() {} |
||||
// init - initialised the device
|
||||
virtual bool init(void) = 0; |
||||
// update - updates device according to timed_updated. Should be
|
||||
// called at 50Hz
|
||||
virtual void update() = 0; |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,315 @@
@@ -0,0 +1,315 @@
|
||||
/*
|
||||
Generic RGBLed driver |
||||
*/ |
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
*/ |
||||
|
||||
|
||||
#include <AP_HAL.h> |
||||
#include <AP_GPS.h> |
||||
#include "Led.h" |
||||
#include "AP_Notify.h" |
||||
|
||||
extern const AP_HAL::HAL& hal; |
||||
|
||||
RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim): |
||||
_led_off(led_off), |
||||
_led_bright(led_bright), |
||||
_led_medium(led_medium), |
||||
_led_dim(led_dim) |
||||
|
||||
{ |
||||
|
||||
}
|
||||
|
||||
bool RGBLed::init() |
||||
{ |
||||
_healthy = hw_init(); |
||||
return _healthy; |
||||
} |
||||
|
||||
// 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) |
||||
{ |
||||
// return immediately if not enabled
|
||||
if (!_healthy) { |
||||
return; |
||||
} |
||||
|
||||
if (red != _red_curr || |
||||
green != _green_curr || |
||||
blue != _blue_curr) { |
||||
// call the hardware update routine
|
||||
if (hw_set_rgb(red, green, blue)) { |
||||
_red_curr = red; |
||||
_green_curr = green; |
||||
_blue_curr = blue; |
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
// _scheduled_update - updates _red, _green, _blue according to notify flags
|
||||
void RGBLed::update_colours(void) |
||||
{ |
||||
uint8_t brightness = _led_bright; |
||||
// slow rate from 50Hz to 10hz
|
||||
counter++; |
||||
if (counter < 5) { |
||||
return; |
||||
} |
||||
|
||||
// reset counter
|
||||
counter = 0; |
||||
|
||||
// move forward one step
|
||||
step++; |
||||
if (step >= 10) { |
||||
step = 0; |
||||
} |
||||
|
||||
// use dim light when connected through USB
|
||||
if (hal.gpio->usb_connected()) { |
||||
brightness = _led_dim; |
||||
} |
||||
|
||||
// initialising pattern
|
||||
if (AP_Notify::flags.initialising) { |
||||
if (step & 1) { |
||||
// odd steps display red light
|
||||
_red_des = brightness; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
} else { |
||||
// even display blue light
|
||||
_red_des = _led_off; |
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
} |
||||
|
||||
// exit so no other status modify this pattern
|
||||
return; |
||||
} |
||||
|
||||
// save trim and esc calibration pattern
|
||||
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { |
||||
switch(step) { |
||||
case 0: |
||||
case 3: |
||||
case 6: |
||||
// red on
|
||||
_red_des = brightness; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
break; |
||||
|
||||
case 1: |
||||
case 4: |
||||
case 7: |
||||
// blue on
|
||||
_red_des = _led_off; |
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
break; |
||||
|
||||
case 2: |
||||
case 5: |
||||
case 8: |
||||
// green on
|
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = brightness; |
||||
break; |
||||
|
||||
case 9: |
||||
// all off
|
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
break; |
||||
} |
||||
// exit so no other status modify this pattern
|
||||
return; |
||||
} |
||||
|
||||
// radio and battery failsafe patter: flash yellow
|
||||
// gps failsafe pattern : flashing yellow and blue
|
||||
// baro glitching pattern : flashing yellow and purple
|
||||
// ekf_bad pattern : flashing yellow and red
|
||||
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery || |
||||
AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching || |
||||
AP_Notify::flags.baro_glitching || |
||||
AP_Notify::flags.ekf_bad) { |
||||
switch(step) { |
||||
case 0: |
||||
case 1: |
||||
case 2: |
||||
case 3: |
||||
case 4: |
||||
// yellow on
|
||||
_red_des = brightness; |
||||
_blue_des = _led_off; |
||||
_green_des = brightness; |
||||
break; |
||||
case 5: |
||||
case 6: |
||||
case 7: |
||||
case 8: |
||||
case 9: |
||||
if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) { |
||||
// blue on for gps failsafe or glitching
|
||||
_red_des = _led_off; |
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
} else if (AP_Notify::flags.baro_glitching) { |
||||
// purple on if baro glitching
|
||||
_red_des = brightness; |
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
} else if (AP_Notify::flags.ekf_bad) { |
||||
// red on if ekf bad
|
||||
_red_des = brightness; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
}else{ |
||||
// all off for radio or battery failsafe
|
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
} |
||||
// exit so no other status modify this pattern
|
||||
return; |
||||
} |
||||
|
||||
// solid green or flashing green if armed
|
||||
if (AP_Notify::flags.armed) { |
||||
// solid green if armed with GPS 3d lock
|
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { |
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = brightness; |
||||
}else{ |
||||
// solid blue if armed with no GPS lock
|
||||
_red_des = _led_off; |
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
} |
||||
return; |
||||
}else{ |
||||
// double flash yellow if failing pre-arm checks
|
||||
if (!AP_Notify::flags.pre_arm_check) { |
||||
switch(step) { |
||||
case 0: |
||||
case 1: |
||||
case 4: |
||||
case 5: |
||||
// yellow on
|
||||
_red_des = brightness; |
||||
_blue_des = _led_off; |
||||
_green_des = brightness; |
||||
break; |
||||
case 2: |
||||
case 3: |
||||
case 6: |
||||
case 7: |
||||
case 8: |
||||
case 9: |
||||
// all off
|
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
break; |
||||
} |
||||
}else{ |
||||
// flashing green if disarmed with GPS 3d lock
|
||||
// flashing blue if disarmed with no gps lock
|
||||
switch(step) { |
||||
case 0: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = brightness; |
||||
} |
||||
break; |
||||
case 1: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
case 2: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = brightness; |
||||
} |
||||
break; |
||||
case 3: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
case 4: |
||||
_red_des = _led_off; |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { |
||||
// flashing green if disarmed with GPS 3d lock
|
||||
_blue_des = _led_off; |
||||
_green_des = brightness; |
||||
}else{ |
||||
// flashing blue if disarmed with no gps lock
|
||||
_blue_des = brightness; |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
case 5: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
|
||||
case 6: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = brightness; |
||||
} |
||||
break; |
||||
|
||||
case 7: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = _led_off; |
||||
} |
||||
break; |
||||
case 8: |
||||
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { |
||||
_green_des = brightness; |
||||
} |
||||
break; |
||||
case 9: |
||||
// all off
|
||||
_red_des = _led_off; |
||||
_blue_des = _led_off; |
||||
_green_des = _led_off; |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
// update - updates led according to timed_updated. Should be called
|
||||
// at 50Hz
|
||||
void RGBLed::update() |
||||
{ |
||||
// return immediately if not enabled
|
||||
if (!_healthy) { |
||||
return; |
||||
} |
||||
update_colours(); |
||||
set_rgb(_red_des, _green_des, _blue_des); |
||||
} |
||||
@ -0,0 +1,63 @@
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* AP_Notify Library.
|
||||
* based upon a prototype library by David "Buzz" Bussenschutt. |
||||
*/ |
||||
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#ifndef __RGBLED_H__ |
||||
#define __RGBLED_H__ |
||||
|
||||
#include <AP_HAL.h> |
||||
#include "Led.h" |
||||
|
||||
class RGBLed: public Led { |
||||
public: |
||||
RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim); |
||||
|
||||
// init - initialised the LED
|
||||
virtual bool init(void); |
||||
|
||||
// healthy - returns true if the LED is operating properly
|
||||
virtual bool healthy() { return _healthy; } |
||||
|
||||
// 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); |
||||
|
||||
// update - updates led according to timed_updated. Should be
|
||||
// called at 50Hz
|
||||
virtual void update(); |
||||
|
||||
protected: |
||||
// methods implemented in hardware specific classes
|
||||
virtual bool hw_init(void) = 0; |
||||
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0; |
||||
|
||||
// meta-data common to all hw devices
|
||||
uint8_t counter; |
||||
uint8_t step; |
||||
bool _healthy; // true if the LED is operating properly
|
||||
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
|
||||
uint8_t _led_off; |
||||
uint8_t _led_bright; |
||||
uint8_t _led_medium; |
||||
uint8_t _led_dim; |
||||
private: |
||||
virtual void update_colours(); |
||||
}; |
||||
|
||||
#endif //__RGBLED_H__
|
||||
Loading…
Reference in new issue