HighSpeedPWM(unsigned int pwm_rate) :
_pwm_rate(pwm_rate), _pwm_period(1.0f / (float) pwm_rate)
{
/* There's some sleeps in here that might not all be necessary... */
printf("Resetting GPIO\n");
gpioReset();
usleep(500000);
/* Magic number borrowed from src/drivers/px4fmu/fmu.cpp */
printf("Initializing PWM\n");
up_pwm_servo_init(0x3f);
usleep(500000);
/* Set PWM Rate for all outputs */
printf("Setting PWM rate\n");
if(up_pwm_servo_set_rate(_pwm_rate) != OK) {
printf("set rate failed");
return;
}
usleep(500000);
printf("Arming PWM\n");
up_pwm_servo_arm(true);
usleep(500000);
}
static void
comms_handle_command(const void *buffer, size_t length)
{
const struct px4io_command *cmd = (struct px4io_command *)buffer;
if (length != sizeof(*cmd))
return;
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->output_control[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if (system_state.arm_ok && !cmd->arm_ok)
system_state.armed = false;
system_state.arm_ok = cmd->arm_ok;
system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
system_state.manual_override_ok = cmd->manual_override_ok;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received_time = hrt_absolute_time();
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
/* reject faster than 500 Hz updates */
if (new_servo_rate > 500) {
new_servo_rate = 500;
}
/* reject slower than 50 Hz updates */
if (new_servo_rate < 50) {
new_servo_rate = 50;
}
if (system_state.servo_rate != new_servo_rate) {
up_pwm_servo_set_rate(new_servo_rate);
system_state.servo_rate = new_servo_rate;
}
/*
* update servo values immediately.
* the updates are done in addition also
* in the mainloop, since this function will only
* update with a connected FMU.
*/
mixer_tick();
/* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
