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

HAL_PX4: fixed setting of rcout frequency on alt channels

This commit is contained in:
Andrew Tridgell 2016-01-05 07:17:44 +11:00
parent 224b2e2dda
commit 526fb65dd1
2 changed files with 42 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -85,19 +85,22 @@ void PX4RCOutput::_init_alt_channels(void)
} }
} }
void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) /*
set output frequency on outputs associated with fd
*/
void PX4RCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
{ {
// we can't set this per channel yet // we can't set this per channel
if (freq_hz > 50) { if (freq_hz > 50) {
// we're being asked to set the alt rate // we're being asked to set the alt rate
if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) { 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); hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
return; return;
} }
_freq_hz = freq_hz; _freq_hz = freq_hz;
} }
/* work out the new rate mask. The PX4IO board has 3 groups of servos. /* work out the new rate mask. The outputs have 3 groups of servos.
Group 0: channels 0 1 Group 0: channels 0 1
Group 1: channels 4 5 6 7 Group 1: channels 4 5 6 7
@ -133,11 +136,31 @@ void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
} }
} }
if (ioctl(_pwm_fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) { if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask); hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
} }
} }
/*
set output frequency
*/
void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
// greater than 400 doesn't give enough room at higher periods for
// the down pulse
if (freq_hz > 400) {
freq_hz = 400;
}
uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
uint32_t alt_mask = chmask >> _servo_count;
if (primary_mask && _pwm_fd != -1) {
set_freq_fd(_pwm_fd, primary_mask, freq_hz);
}
if (alt_mask && _alt_fd != -1) {
set_freq_fd(_alt_fd, alt_mask, freq_hz);
}
}
uint16_t PX4RCOutput::get_freq(uint8_t ch) uint16_t PX4RCOutput::get_freq(uint8_t ch)
{ {
if (_rate_mask & (1U<<ch)) { if (_rate_mask & (1U<<ch)) {

27
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -12,19 +12,19 @@
class PX4::PX4RCOutput : public AP_HAL::RCOutput class PX4::PX4RCOutput : public AP_HAL::RCOutput
{ {
public: public:
void init(); void init() override;
void set_freq(uint32_t chmask, uint16_t freq_hz); void set_freq(uint32_t chmask, uint16_t freq_hz) override;
uint16_t get_freq(uint8_t ch); uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch); void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch); void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us); void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch); uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len); void read(uint16_t* period_us, uint8_t len) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us); void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us); void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void); bool force_safety_on(void) override;
void force_safety_off(void); void force_safety_off(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) { void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
_esc_pwm_min = min_pwm; _esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm; _esc_pwm_max = max_pwm;
} }
@ -59,6 +59,7 @@ private:
void _init_alt_channels(void); void _init_alt_channels(void);
void _publish_actuators(void); void _publish_actuators(void);
void _arm_actuators(bool arm); void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
}; };
#endif // __AP_HAL_PX4_RCOUTPUT_H__ #endif // __AP_HAL_PX4_RCOUTPUT_H__