void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) { // soft-armed overrides passed in armed status if (!hal.util->get_soft_armed()) { armed = false; _throttle = 0.0f; } // sanity check parameters sanity_check_parameters(); // clear and set limits based on input (limit flags may be set again by output_regular or output_skid_steering methods) set_limits_from_input(armed, _steering, _throttle); // slew limit throttle slew_limit_throttle(dt); // output for regular steering/throttle style frames output_regular(armed, ground_speed, _steering, _throttle); // output for omni style frames output_omni(armed, _steering, _throttle); // output for skid steering style frames output_skid_steering(armed, _steering, _throttle); // send values to the PWM timers for output SRV_Channels::calc_pwm(); SRV_Channels::cork(); SRV_Channels::output_ch_all(); SRV_Channels::push(); }
// output for omni style frames void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float lateral) { if (!has_lateral_control()) { return; } // clear and set limits based on input set_limits_from_input(armed, steering, throttle); // constrain steering steering = constrain_float(steering, -4500.0f, 4500.0f); if (armed) { // scale throttle, steering and lateral to -1 ~ 1 const float scaled_throttle = throttle / 100.0f; const float scaled_steering = steering / 4500.0f; const float scaled_lateral = lateral / 100.0f; // calculate desired vehicle speed and direction const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(scaled_lateral*scaled_lateral)); const float theta = atan2f(scaled_throttle,scaled_lateral); // calculate X and Y vectors using the following the equations: vx = cos(theta) * magnitude and vy = sin(theta) * magnitude const float Vx = -(cosf(theta)*magnitude); const float Vy = -(sinf(theta)*magnitude); // calculate output throttle for each motor. Output is multiplied by 0.5 to bring the range generally within -1 ~ 1 // First wheel (motor 1) moves only parallel to x-axis so only X component is taken. Normal range is -2 ~ 2 with the steering // motor_2 and motor_3 utilizes both X and Y components. // safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory float motor_1 = 0.5 * ((-Vx) + scaled_steering); float motor_2 = 0.5 * (((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering); float motor_3 = 0.5 * (((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering); // apply constraints motor_1 = constrain_float(motor_1, -1.0f, 1.0f); motor_2 = constrain_float(motor_2, -1.0f, 1.0f); motor_3 = constrain_float(motor_3, -1.0f, 1.0f); // scale back and send pwm value to each motor output_throttle(SRV_Channel::k_motor1, 100.0f * motor_1); output_throttle(SRV_Channel::k_motor2, 100.0f * motor_2); output_throttle(SRV_Channel::k_motor3, 100.0f * motor_3); } else { // handle disarmed case if (_disarm_disable_pwm) { SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); } else { SRV_Channels::set_output_limit(SRV_Channel::k_motor1, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_motor2, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_motor3, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); } } }
// output to regular steering and throttle channels void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle) { // output to throttle channels if (armed) { if (_scale_steering) { // vectored thrust handling if (have_vectored_thrust()) { if (fabsf(throttle) > _vector_throttle_base) { // scale steering down linearly as throttle increases above _vector_throttle_base steering *= constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f); } } else { // scale steering down as speed increase above MOT_SPD_SCA_BASE (1 m/s default) if (is_positive(_speed_scale_base) && (fabsf(ground_speed) > _speed_scale_base)) { steering *= (_speed_scale_base / fabsf(ground_speed)); } else { // regular steering rover at low speed so set limits to stop I-term build-up in controllers if (!have_skid_steering()) { limit.steer_left = true; limit.steer_right = true; } } // reverse steering direction when backing up if (is_negative(ground_speed)) { steering *= -1.0f; } } } else { // reverse steering direction when backing up if (is_negative(throttle)) { steering *= -1.0f; } } output_throttle(SRV_Channel::k_throttle, throttle); } else { // handle disarmed case if (_disarm_disable_pwm) { SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); } else { SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); } } // clear and set limits based on input // we do this here because vectored thrust or speed scaling may have reduced steering request set_limits_from_input(armed, steering, throttle); // constrain steering steering = constrain_float(steering, -4500.0f, 4500.0f); // always allow steering to move SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering); }
// output to skid steering channels void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle) { if (!have_skid_steering()) { return; } // clear and set limits based on input set_limits_from_input(armed, steering, throttle); // constrain steering steering = constrain_float(steering, -4500.0f, 4500.0f); // handle simpler disarmed case if (!armed) { if (_disarm_disable_pwm) { SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); } else { SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); } return; } // skid steering mixer float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1 float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1 // apply constraints steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f); throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f); // check for saturation and scale back throttle and steering proportionally const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled); if (saturation_value > 1.0f) { steering_scaled = steering_scaled / saturation_value; throttle_scaled = throttle_scaled / saturation_value; } // add in throttle and steering const float motor_left = throttle_scaled + steering_scaled; const float motor_right = throttle_scaled - steering_scaled; // send pwm value to each motor output_throttle(SRV_Channel::k_throttleLeft, 100.0f * motor_left); output_throttle(SRV_Channel::k_throttleRight, 100.0f * motor_right); }
// output for custom configurations void AP_MotorsUGV::output_custom_config(bool armed, float steering, float throttle, float lateral) { // exit immediately if the frame type is set to UNDEFINED if (rover.get_frame_type() == FRAME_TYPE_UNDEFINED) { return; } if (armed) { // clear and set limits based on input set_limits_from_input(armed, steering, throttle); // constrain steering steering = constrain_float(steering, -4500.0f, 4500.0f); // scale throttle, steering and lateral inputs to -1 to 1 const float scaled_throttle = throttle / 100.0f; const float scaled_steering = steering / 4500.0f; const float scaled_lateral = lateral / 100.0f; float thr_str_ltr_out; float thr_str_ltr_max = 1; for (uint8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) { thr_str_ltr_out = (scaled_throttle * _throttle_factor[i]) + (scaled_steering * _steering_factor[i]) + (scaled_lateral * _lateral_factor[i]); if (fabsf(thr_str_ltr_out) > thr_str_ltr_max) { thr_str_ltr_max = fabsf(thr_str_ltr_out); } float output_vectored = (thr_str_ltr_out / thr_str_ltr_max); // send output for each motor output_throttle(SRV_Channels::get_motor_function(i), 100.0f * output_vectored); } } else { // handle disarmed case if (_disarm_disable_pwm) { for (uint8_t i=0; i<_motors_num; i++) { SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM); } } else { for (uint8_t i=0; i<_motors_num; i++) { SRV_Channels::set_output_limit(SRV_Channels::get_motor_function(i), SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); } } } }