// 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 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);
}
// test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command
bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
{
// check if the motor_seq is valid
if (motor_seq > MOTOR_TEST_THROTTLE_RIGHT) {
return false;
}
pct = constrain_float(pct, -100.0f, 100.0f);
switch (motor_seq) {
case MOTOR_TEST_THROTTLE: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
return false;
}
output_throttle(SRV_Channel::k_throttle, pct);
break;
}
case MOTOR_TEST_STEERING: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
return false;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, pct * 45.0f);
break;
}
case MOTOR_TEST_THROTTLE_LEFT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
return false;
}
output_throttle(SRV_Channel::k_throttleLeft, pct);
break;
}
case MOTOR_TEST_THROTTLE_RIGHT: {
if (!SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
return false;
}
output_throttle(SRV_Channel::k_throttleRight, pct);
break;
}
default:
return false;
}
SRV_Channels::calc_pwm();
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
return true;
}
// test steering or throttle output as a percentage of the total (range -100 to +100)
// used in response to DO_MOTOR_TEST mavlink command
bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct)
{
// check if the motor_seq is valid
if (motor_seq >= MOTOR_TEST_LAST) {
return false;
}
pct = constrain_float(pct, -100.0f, 100.0f);
switch (motor_seq) {
case MOTOR_TEST_THROTTLE: {
if (SRV_Channels::function_assigned(SRV_Channel::k_motor1)) {
output_throttle(SRV_Channel::k_motor1, pct);
}
if (SRV_Channels::function_assigned(SRV_Channel::k_throttle)) {
output_throttle(SRV_Channel::k_throttle, pct);
}
break;
}
case MOTOR_TEST_STEERING: {
if (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) {
output_throttle(SRV_Channel::k_motor2, pct);
}
if (SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, pct * 45.0f);
}
break;
}
case MOTOR_TEST_THROTTLE_LEFT: {
if (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) {
output_throttle(SRV_Channel::k_motor3, pct);
}
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft)) {
output_throttle(SRV_Channel::k_throttleLeft, pct);
}
break;
}
case MOTOR_TEST_THROTTLE_RIGHT: {
if (SRV_Channels::function_assigned(SRV_Channel::k_motor4)) {
output_throttle(SRV_Channel::k_motor4, pct);
}
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
output_throttle(SRV_Channel::k_throttleRight, pct);
}
break;
}
case MOTOR_TEST_MAINSAIL: {
if (SRV_Channels::function_assigned(SRV_Channel::k_mainsail_sheet)) {
SRV_Channels::set_output_scaled(SRV_Channel::k_mainsail_sheet, pct);
}
break;
}
case MOTOR_TEST_LAST:
return false;
}
SRV_Channels::calc_pwm();
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
return true;
}
// output to skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
{
if (!have_skid_steering()) {
return;
}
// 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;
}
// reverse steering direction if throttle is negative to mimic regular rovers
const float steering_dir = is_negative(throttle_scaled) ? -1.0f : 1.0f;
// add in throttle and steering
const float motor_left = throttle_scaled + (steering_dir * steering_scaled);
const float motor_right = throttle_scaled - (steering_dir * 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 omni style frames
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
{
if (!is_omni_rover()) {
return;
}
if (armed) {
// scale throttle and steering to a 1000 to 2000 range for motor throttle calculations
const float scaled_throttle = (throttle - (100)) * (2000 - 1000) / (-100 - (100)) + 1000;
const float scaled_steering = (steering - (-4500)) * (2000 - 1000) / (4500 - (-4500)) + 1000;
// calculate desired vehicle speed and direction
// 1500 is a place-holder value for lateral movement input
const float magnitude = safe_sqrt((scaled_throttle*scaled_throttle)+(1500*1500));
const float theta = atan2f(scaled_throttle,1500);
// calculate X and Y vectors using the following the equations: vx = cos(?) * magnitude and vy = sin(?) * magnitude
const float Vx = -(cosf(theta)*magnitude);
const float Vy = -(sinf(theta)*magnitude);
// calculate output throttle for each motor and scale it back to a -100 to 100 range
// calculations are done using the following equations: MOTOR1 = –vx, MOTOR2 = 0.5 * v – v(3/2) * vy, MOTOR 3 = 0.5 * vx + v(3/2) * vy
// safe_sqrt((3)/2) used because the motors are 120 degrees apart in the frame, this setup is mandatory
const int16_t motor_1 = (((-Vx) + scaled_steering) - (2500)) * (100 - (-100)) / (3500 - (2500)) + (-100);
const int16_t motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (1121)) * (100 - (-100)) / (2973 - (1121)) + (-100);
const int16_t motor_3 = ((((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + scaled_steering) - (-1468)) * (100 - (-100)) / (383 - (-1468)) + (-100);
// send pwm value to each motor
output_throttle(SRV_Channel::k_motor1, motor_1);
output_throttle(SRV_Channel::k_motor2, motor_2);
output_throttle(SRV_Channel::k_motor3, 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 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);
}
}
}
}
// 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 1m/s
if (fabsf(ground_speed) > 1.0f) {
steering *= (1.0f / 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 output if 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);
}
}
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
// output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
{
// output to throttle channels
if (armed) {
// vectored thrust handling
if (have_vectored_thrust() && (fabsf(throttle) > _vector_throttle_base)) {
// scale steering down linearly as throttle increases above _vector_throttle_base
const float steering_scalar = constrain_float(_vector_throttle_base / fabsf(throttle), 0.0f, 1.0f);
steering *= steering_scalar;
}
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);
}
}
// always allow steering to move
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}
