Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: improved oneshot support 

this now supports oneshot properly on both IO and FMU
This commit is contained in:
Andrew Tridgell 2016-04-16 07:23:49 +10:00
parent ebd89bd078
commit c9dfccfb26
2 changed files with 22 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -102,6 +102,14 @@ void PX4RCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
// we can't set this per channel
if (freq_hz > 50) {
// we're being asked to set the alt rate
if (_output_mode == MODE_PWM_ONESHOT) {
/*
set a 1Hz update for oneshot. This periodic output will
never actually trigger, instead we will directly trigger
the pulse after each push()
*/
freq_hz = 1;
}
if (ioctl(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;
@ -160,10 +168,6 @@ void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
if (freq_hz > 400) {
freq_hz = 400;
}
if (freq_hz == 400) {
// remember mask for fast channels
_fast_channel_mask = chmask;
}
uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
uint32_t alt_mask = chmask >> _servo_count;
if (primary_mask && _pwm_fd != -1) {
@ -267,6 +271,11 @@ void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
if (ch >= _max_channel) {
_max_channel = ch + 1;
}
/*
only mark an update is needed if there has been a change, or we
are in oneshot mode. In oneshot mode we always need to send the
output
*/
if (period_us != _period[ch] ||
_output_mode == MODE_PWM_ONESHOT) {
_period[ch] = period_us;
@ -478,44 +487,18 @@ void PX4RCOutput::push()
if (_output_mode == MODE_PWM_ONESHOT) {
// run timer immediately in oneshot mode
_send_outputs();
if (_fast_channel_mask != 0) {
// this is a crude way of triggering immediate output
_trigger_fast_output();
}
}
}
void PX4RCOutput::_timer_tick(void)
{
if (_output_mode != MODE_PWM_ONESHOT) {
/* in oneshot mode the timer does nothing as the outputs are
* sent from push() */
_send_outputs();
}
}
/*
trigger immediate output on fast channels. This only works on FMU,
not IO. It takes advantage of the way the rate update works on FMU
to trigger a oneshot output
*/
void PX4RCOutput::_trigger_fast_output(void)
{
uint32_t primary_mask = _fast_channel_mask & ((1UL<<_servo_count)-1);
uint32_t alt_mask = _fast_channel_mask >> _servo_count;
if (_alt_fd == -1) {
// we're on a FMU only board
if (primary_mask) {
set_freq_fd(_pwm_fd, primary_mask, 400);
set_freq_fd(_pwm_fd, primary_mask, 51);
}
} else {
// we're on a board with px4io
if (alt_mask) {
set_freq_fd(_alt_fd, alt_mask, 400);
set_freq_fd(_alt_fd, alt_mask, 51);
}
}
}
/*
enable sbus output
*/
@ -548,8 +531,15 @@ void PX4RCOutput::set_output_mode(enum output_mode mode)
_output_mode = mode;
if (_output_mode == MODE_PWM_ONESHOT) {
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
}
set_freq(0xFFFF, 51);
} else {
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0);
}
}
}

3
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -61,7 +61,6 @@ private:
orb_advert_t _actuator_armed_pub = NULL;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;
uint16_t _fast_channel_mask;
void _init_alt_channels(void);
void _publish_actuators(void);
@ -70,6 +69,4 @@ private:
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
void _trigger_fast_output();
};