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); }