mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
Rover: added is_omni_rover function
This commit is contained in:
parent
cc50fff51f
commit
d7e01d83d6
APMrover2
@ -180,6 +180,17 @@ bool AP_MotorsUGV::have_skid_steering() const
|
||||
return false;
|
||||
}
|
||||
|
||||
// returns true if omni rover
|
||||
bool AP_MotorsUGV::is_omni_rover() const
|
||||
{
|
||||
if (SRV_Channels::function_assigned(SRV_Channel::k_motor1) &&
|
||||
SRV_Channels::function_assigned(SRV_Channel::k_motor2) &&
|
||||
SRV_Channels::function_assigned(SRV_Channel::k_motor3)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_MotorsUGV::output(bool armed, float dt)
|
||||
{
|
||||
// soft-armed overrides passed in armed status
|
||||
@ -200,6 +211,9 @@ void AP_MotorsUGV::output(bool armed, float dt)
|
||||
// output for regular steering/throttle style frames
|
||||
output_regular(armed, _steering, _throttle);
|
||||
|
||||
// output for omni style frames
|
||||
output_omni(armed, _throttle, _steering);
|
||||
|
||||
// output for skid steering style frames
|
||||
output_skid_steering(armed, _steering, _throttle);
|
||||
|
||||
@ -332,6 +346,14 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// check if only one of the omni rover outputs has been configured
|
||||
if ((SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor2)) ||
|
||||
(SRV_Channels::function_assigned(SRV_Channel::k_motor1)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3)) ||
|
||||
(SRV_Channels::function_assigned(SRV_Channel::k_motor2)) != (SRV_Channels::function_assigned(SRV_Channel::k_motor3))) {
|
||||
if (report) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check motor 1, motor2 and motor3 config");
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -396,6 +418,48 @@ void AP_MotorsUGV::output_regular(bool armed, float steering, float throttle)
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
|
||||
}
|
||||
|
||||
// output for omni style frames
|
||||
void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle)
|
||||
{
|
||||
if(!(is_omni_rover())) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (armed) {
|
||||
throttle = (_throttle - (100)) * (2000 - 1000) / (-100 - (100)) + 1000;
|
||||
steering = (_steering - (-4500)) * (2000 - 1000) / (4500 - (-4500)) + 1000;
|
||||
|
||||
int16_t motor_1, motor_2, motor_3;
|
||||
float Vx, Vy, magnitude, theta;
|
||||
|
||||
magnitude = safe_sqrt((throttle*throttle)+(1500*1500));
|
||||
theta = atan2f(throttle,1500);
|
||||
Vx = -(cosf(theta)*magnitude);
|
||||
Vy = -(sinf(theta)*magnitude);
|
||||
|
||||
motor_1 = (((-Vx) + steering) - (2500)) * (2000 - (1000)) / (3500 - (2500)) + (1000);
|
||||
motor_2 = ((((0.5*Vx)-((safe_sqrt(3)/2)*Vy)) + steering) - (1121)) * (2000 - (1000)) / (2973 - (1121)) + (1000);
|
||||
motor_3 = ((((0.5*Vx)+((safe_sqrt(3)/2)*Vy)) + steering) - (-1468)) * (2000 - (1000)) / (383 - (-1468)) + (1000);
|
||||
|
||||
SRV_Channels::set_output_pwm(SRV_Channel::k_motor1, motor_1);
|
||||
SRV_Channels::set_output_pwm(SRV_Channel::k_motor2, motor_2);
|
||||
SRV_Channels::set_output_pwm(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 skid steering channels
|
||||
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
|
||||
{
|
||||
|
@ -45,6 +45,9 @@ public:
|
||||
// true if vehicle is capable of skid steering
|
||||
bool have_skid_steering() const;
|
||||
|
||||
//true if vehicle is an omni rover
|
||||
bool is_omni_rover() const;
|
||||
|
||||
// true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
|
||||
bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
|
||||
|
||||
@ -83,6 +86,9 @@ protected:
|
||||
// output to regular steering and throttle channels
|
||||
void output_regular(bool armed, float steering, float throttle);
|
||||
|
||||
// output for omni style frames
|
||||
void output_omni(bool armed, float steering, float throttle);
|
||||
|
||||
// output to skid steering channels
|
||||
void output_skid_steering(bool armed, float steering, float throttle);
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user