motor_state VtolType::set_motor_state(const motor_state current_state, const motor_state next_state, const int value) { struct pwm_output_values pwm_values = {}; pwm_values.channel_count = _params->vtol_motor_count; // per default all motors are running for (int i = 0; i < _params->vtol_motor_count; i++) { pwm_values.values[i] = _max_mc_pwm_values.values[i]; } switch (next_state) { case ENABLED: break; case DISABLED: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = _disarmed_pwm_values.values[i]; } } break; case IDLE: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = _params->idle_pwm_mc; } } break; case VALUE: for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = value; } } break; } if (apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MAXIMUM)) { return next_state; } else { return current_state; } }
void Tiltrotor::set_rear_motor_state(rear_motor_state state) { int pwm_value = PWM_DEFAULT_MAX; // map desired rear rotor state to max allowed pwm signal switch (state) { case ENABLED: pwm_value = PWM_DEFAULT_MAX; _rear_motors = ENABLED; break; case DISABLED: pwm_value = PWM_LOWEST_MAX; _rear_motors = DISABLED; break; case IDLE: pwm_value = _params->idle_pwm_mc; _rear_motors = IDLE; break; } int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) {PX4_WARN("can't open %s", dev);} ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; } else { pwm_values.values[i] = PWM_DEFAULT_MAX; } pwm_values.channel_count = _params->vtol_motor_count; } ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); if (ret != OK) {PX4_WARN("failed setting max values");} px4_close(fd); }