Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
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,12 +57,12 @@ 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 */
@ -68,19 +97,18 @@ void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
*/ */
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;

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