Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: support 12 output channels with PX4IO

Browse Source 
first 8 on IO board, next 4 on FMU outputs
This commit is contained in:
Andrew Tridgell 2013-04-25 19:59:30 +10:00
parent 3b0250b414
commit 709615159a
2 changed files with 97 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -26,23 +26,96 @@ void PX4RCOutput::init(void* unused)
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming\n");
}
_rate_mask = 0;
_alt_fd = -1;
_servo_count = 0;
_alt_servo_count = 0;
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
/* if we have 8 servos, then we are attached to PX4IO. In that
* case, we want to open the PX4FMU driver for the 4 additional
* channels
*/
if (_servo_count <= 4) {
return;
}
_alt_fd = open("/dev/px4fmu", O_RDWR);
if (_alt_fd == -1) {
hal.console->printf("RCOutput: failed to open /dev/px4fmu");
return;
}
if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
}
if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
}
void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
if (freq_hz == _freq_hz) {
// avoid the ioctl() if possible
return;
}
// we can't set this per channel yet
if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) == 0) {
if (freq_hz > 50) {
// we're being asked to set the alt rate
if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
return;
}
_freq_hz = freq_hz;
}
/* work out the new rate mask. The PX4IO board has 3 groups of servos.
Group 0: channels 0 1
Group 1: channels 4 5 6 7
Group 2: channels 2 3
Channels within a group must be set to the same rate.
For the moment we never set the channels above 8 to more than
50Hz
*/
if (freq_hz > 50) {
// we are setting high rates on the given channels
_rate_mask |= chmask & 0xFF;
if (_rate_mask & 0x3) {
_rate_mask |= 0x3;
}
if (_rate_mask & 0xc) {
_rate_mask |= 0xc;
}
if (_rate_mask & 0xF0) {
_rate_mask |= 0xF0;
}
} else {
// we are setting low rates on the given channels
if (chmask & 0x3) {
_rate_mask &= ~0x3;
}
if (chmask & 0xc) {
_rate_mask &= ~0xc;
}
if (chmask & 0xf0) {
_rate_mask &= ~0xf0;
}
}
if (ioctl(_pwm_fd, PWM_SERVO_SELECT_UPDATE_RATE, _rate_mask) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
}
}
uint16_t PX4RCOutput::get_freq(uint8_t ch)
{
return _freq_hz;
if (_rate_mask & (1<<ch)) {
return _freq_hz;
}
return 50;
}
void PX4RCOutput::enable_ch(uint8_t ch)
@ -67,7 +140,7 @@ void PX4RCOutput::disable_mask(uint32_t chmask)
void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
if (ch >= _servo_count + _alt_servo_count) {
return;
}
if (ch >= _max_channel) {
@ -114,7 +187,19 @@ void PX4RCOutput::_timer_tick(void)
if (_need_update && _pwm_fd != -1) {
_need_update = false;
perf_begin(_perf_rcout);
::write(_pwm_fd, _period, _max_channel*sizeof(_period[0]));
if (_max_channel <= _servo_count) {
::write(_pwm_fd, _period, _max_channel*sizeof(_period[0]));
} else {
// we're using both sets of outputs
::write(_pwm_fd, _period, _servo_count*sizeof(_period[0]));
if (_alt_fd != -1 && _alt_servo_count > 0) {
uint8_t n = _max_channel - _servo_count;
if (n > _alt_servo_count) {
n = _alt_servo_count;
}
::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
}
}
perf_end(_perf_rcout);
_last_output = now;
}

4
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -26,12 +26,16 @@ public:
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[PX4_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
perf_counter_t _perf_rcout;
uint32_t _last_output;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
};
#endif // __AP_HAL_PX4_RCOUTPUT_H__