Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-08 06:34:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: fixed style in RCOutput_Navio

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-02-26 14:53:45 +03:00 committed by Andrew Tridgell
parent 024825f95a
commit e6c2977da2
2 changed files with 62 additions and 63 deletions

94
libraries/AP_HAL_Linux/RCOutput_Navio.cpp
View File

@ -15,6 +15,35 @@
#include <stdint.h> #include <stdint.h>
#include <math.h> #include <math.h>
#define PCA9685_ADDRESS 0x40 // All address pins low, Navio default
#define PCA9685_RA_MODE1 0x00
#define PCA9685_RA_MODE2 0x01
#define PCA9685_RA_LED0_ON_L 0x06
#define PCA9685_RA_LED0_ON_H 0x07
#define PCA9685_RA_LED0_OFF_L 0x08
#define PCA9685_RA_LED0_OFF_H 0x09
#define PCA9685_RA_ALL_LED_ON_L 0xFA
#define PCA9685_RA_ALL_LED_ON_H 0xFB
#define PCA9685_RA_ALL_LED_OFF_L 0xFC
#define PCA9685_RA_ALL_LED_OFF_H 0xFD
#define PCA9685_RA_PRE_SCALE 0xFE
#define PCA9685_MODE1_RESTART_BIT (1 << 7)
#define PCA9685_MODE1_EXTCLK_BIT (1 << 6)
#define PCA9685_MODE1_AI_BIT (1 << 5)
#define PCA9685_MODE1_SLEEP_BIT (1 << 4)
#define PCA9685_MODE1_SUB1_BIT (1 << 3)
#define PCA9685_MODE1_SUB2_BIT (1 << 2)
#define PCA9685_MODE1_SUB3_BIT (1 << 1)
#define PCA9685_MODE1_ALLCALL_BIT (1 << 0)
#define PCA9685_ALL_LED_OFF_H_SHUT (1 << 4)
#define PCA9685_MODE2_INVRT_BIT (1 << 4)
#define PCA9685_MODE2_OCH_BIT (1 << 3)
#define PCA9685_MODE2_OUTDRV_BIT (1 << 2)
#define PCA9685_MODE2_OUTNE1_BIT (1 << 1)
#define PCA9685_MODE2_OUTNE0_BIT (1 << 0)
using namespace Linux; using namespace Linux;
#define PWM_CHAN_COUNT 13 #define PWM_CHAN_COUNT 13
@ -28,14 +57,14 @@ void LinuxRCOutput_Navio::init(void* machtnicht)
if (_i2c_sem == NULL) { if (_i2c_sem == NULL) {
hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio did not get " hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio did not get "
"valid I2C semaphore!")); "valid I2C semaphore!"));
return; // never reached return; /* never reached */
} }
reset_all_channels(); reset_all_channels();
// Set the initial frequency /* Set the initial frequency */
set_freq(0, 50); set_freq(0, 50);
/* Enable PCA9685 PWM */ /* Enable PCA9685 PWM */
enable_pin = hal.gpio->channel(PCA9685_OUTPUT_ENABLE); enable_pin = hal.gpio->channel(PCA9685_OUTPUT_ENABLE);
enable_pin->mode(HAL_GPIO_OUTPUT); enable_pin->mode(HAL_GPIO_OUTPUT);
@ -58,33 +87,32 @@ void LinuxRCOutput_Navio::reset_all_channels()
} }
void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz) void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
{ {
if (!_i2c_sem->take(10)) { if (!_i2c_sem->take(10)) {
return; return;
} }
/* Shutdown before sleeping. /* Shutdown before sleeping.
* see p.14 of PCA9685 product datasheet * see p.14 of PCA9685 product datasheet
*/ */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT); hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
// Put PCA9685 to sleep (required to write prescaler) /* Put PCA9685 to sleep (required to write prescaler) */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT); hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
// Calculate and write prescale value to match frequency /* Calculate and write prescale value to match frequency */
uint8_t prescale = round(24576000.f / 4096.f / freq_hz) - 1; uint8_t prescale = round(24576000.f / 4096.f / freq_hz) - 1;
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale); hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale);
/* Enable external clocking */
// Enable external clocking
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1,
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT); PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT);
// Restart the device to apply new settings and enable auto-incremented write /* Restart the device to apply new settings and enable auto-incremented write */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT); PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_frequency = freq_hz; _frequency = freq_hz;
_i2c_sem->give(); _i2c_sem->give();
} }
@ -95,7 +123,7 @@ uint16_t LinuxRCOutput_Navio::get_freq(uint8_t ch)
void LinuxRCOutput_Navio::enable_ch(uint8_t ch) void LinuxRCOutput_Navio::enable_ch(uint8_t ch)
{ {
} }
void LinuxRCOutput_Navio::disable_ch(uint8_t ch) void LinuxRCOutput_Navio::disable_ch(uint8_t ch)
@ -104,29 +132,29 @@ void LinuxRCOutput_Navio::disable_ch(uint8_t ch)
} }
void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us) void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us)
{ {
if(ch >= PWM_CHAN_COUNT){ if(ch >= PWM_CHAN_COUNT){
return; return;
} }
if (!_i2c_sem->take_nonblocking()) { if (!_i2c_sem->take_nonblocking()) {
return; return;
} }
uint16_t length; uint16_t length;
if (period_us == 0) if (period_us == 0)
length = 0; length = 0;
else else
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1; length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
uint8_t data[2] = {length & 0xFF, length >> 8}; uint8_t data[2] = {length & 0xFF, length >> 8};
uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS, uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS,
PCA9685_RA_LED0_OFF_L + 4 * (ch + 3), PCA9685_RA_LED0_OFF_L + 4 * (ch + 3),
2, 2,
data); data);
_i2c_sem->give(); _i2c_sem->give();
} }
void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t* period_us, uint8_t len) void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t* period_us, uint8_t len)
@ -140,19 +168,19 @@ uint16_t LinuxRCOutput_Navio::read(uint8_t ch)
if (!_i2c_sem->take_nonblocking()) { if (!_i2c_sem->take_nonblocking()) {
return 0; return 0;
} }
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00}; uint8_t data[4] = {0x00, 0x00, 0x00, 0x00};
hal.i2c->readRegisters(PCA9685_ADDRESS, hal.i2c->readRegisters(PCA9685_ADDRESS,
PCA9685_RA_LED0_ON_L + 4 * (ch + 3), PCA9685_RA_LED0_ON_L + 4 * (ch + 3),
4, 4,
data); data);
uint16_t length = data[2] + ((data[3] & 0x0F) << 8); uint16_t length = data[2] + ((data[3] & 0x0F) << 8);
uint16_t period_us = (length + 1) * (1000000.f / _frequency) / 4096.f; uint16_t period_us = (length + 1) * (1000000.f / _frequency) / 4096.f;
_i2c_sem->give(); _i2c_sem->give();
return length == 0 ? 0 : period_us; return length == 0 ? 0 : period_us;
} }
void LinuxRCOutput_Navio::read(uint16_t* period_us, uint8_t len) void LinuxRCOutput_Navio::read(uint16_t* period_us, uint8_t len)

31
libraries/AP_HAL_Linux/RCOutput_Navio.h
View File

@ -4,35 +4,6 @@
#include <AP_HAL_Linux.h> #include <AP_HAL_Linux.h>
#define PCA9685_ADDRESS 0x40 // All address pins low, Navio default
#define PCA9685_RA_MODE1 0x00
#define PCA9685_RA_MODE2 0x01
#define PCA9685_RA_LED0_ON_L 0x06
#define PCA9685_RA_LED0_ON_H 0x07
#define PCA9685_RA_LED0_OFF_L 0x08
#define PCA9685_RA_LED0_OFF_H 0x09
#define PCA9685_RA_ALL_LED_ON_L 0xFA
#define PCA9685_RA_ALL_LED_ON_H 0xFB
#define PCA9685_RA_ALL_LED_OFF_L 0xFC
#define PCA9685_RA_ALL_LED_OFF_H 0xFD
#define PCA9685_RA_PRE_SCALE 0xFE
#define PCA9685_MODE1_RESTART_BIT 1<<7
#define PCA9685_MODE1_EXTCLK_BIT 1<<6
#define PCA9685_MODE1_AI_BIT 1<<5
#define PCA9685_MODE1_SLEEP_BIT 1<<4
#define PCA9685_MODE1_SUB1_BIT 1<<3
#define PCA9685_MODE1_SUB2_BIT 1<<2
#define PCA9685_MODE1_SUB3_BIT 1<<1
#define PCA9685_MODE1_ALLCALL_BIT 1<<0
#define PCA9685_ALL_LED_OFF_H_SHUT 1<<4
#define PCA9685_MODE2_INVRT_BIT 1<<4
#define PCA9685_MODE2_OCH_BIT 1<<3
#define PCA9685_MODE2_OUTDRV_BIT 1<<2
#define PCA9685_MODE2_OUTNE1_BIT 1<<1
#define PCA9685_MODE2_OUTNE0_BIT 1<<0
class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput { class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput {
void init(void* machtnichts); void init(void* machtnichts);
void reset_all_channels(); void reset_all_channels();
@ -49,7 +20,7 @@ private:
void reset(); void reset();
AP_HAL::Semaphore *_i2c_sem; AP_HAL::Semaphore *_i2c_sem;
AP_HAL::DigitalSource *enable_pin; AP_HAL::DigitalSource *enable_pin;
uint16_t _frequency; uint16_t _frequency;
}; };
#endif // __AP_HAL_LINUX_RCOUTPUT_NAVIO_H__ #endif // __AP_HAL_LINUX_RCOUTPUT_NAVIO_H__