Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: added is_omni_rover function

Browse Source
This commit is contained in:
Ammarf 2018-05-07 16:30:44 +09:00 committed by Randy Mackay
parent cc50fff51f
commit d7e01d83d6
2 changed files with 70 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
APMrover2/AP_MotorsUGV.cpp
View File

@ -180,6 +180,17 @@ bool AP_MotorsUGV::have_skid_steering() const
return false; 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) void AP_MotorsUGV::output(bool armed, float dt)
{ {
// soft-armed overrides passed in armed status // 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 for regular steering/throttle style frames
output_regular(armed, _steering, _throttle); output_regular(armed, _steering, _throttle);
// output for omni style frames
output_omni(armed, _throttle, _steering);
// output for skid steering style frames // output for skid steering style frames
output_skid_steering(armed, _steering, _throttle); output_skid_steering(armed, _steering, _throttle);
@ -332,6 +346,14 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const
} }
return false; 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; 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); 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 // output to skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle) void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle)
{ {

6
APMrover2/AP_MotorsUGV.h
View File

@ -45,6 +45,9 @@ public:
// true if vehicle is capable of skid steering // true if vehicle is capable of skid steering
bool have_skid_steering() const; 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) // 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); } bool have_vectored_thrust() const { return is_positive(_vector_throttle_base); }
@ -83,6 +86,9 @@ protected:
// output to regular steering and throttle channels // output to regular steering and throttle channels
void output_regular(bool armed, float steering, float throttle); 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 // output to skid steering channels
void output_skid_steering(bool armed, float steering, float throttle); void output_skid_steering(bool armed, float steering, float throttle);