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

HAL_PX4: implement set_safety_pwm() API 

used to set PWM on motors when disarmed
This commit is contained in:
Andrew Tridgell 2014-01-15 22:25:50 +11:00
parent 5e39b6fb11
commit db1d438e97
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -58,7 +58,7 @@ void PX4RCOutput::init(void* unused)
}
#ifdef PWM_SERVO_SET_ARM_OK
if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
}
#endif
if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
@ -148,6 +148,24 @@ void PX4RCOutput::disable_mask(uint32_t chmask)
// channels are always enabled ...
}
void PX4RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
{
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (uint8_t i=0; i<_servo_count; i++) {
if ((1UL<<i) & chmask) {
pwm_values.values[i] = period_us;
}
pwm_values.channel_count++;
}
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
} else {
hal.console->printf("Setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
}
}
void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= _servo_count + _alt_servo_count) {

1
libraries/AP_HAL_PX4/RCOutput.h
View File

@ -21,6 +21,7 @@ public:
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void set_safety_pwm(uint32_t chmask, uint16_t period_us);
void _timer_tick(void);