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