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

Rover: make omni setup protected in AP_MotorsUGV

Browse Source
This commit is contained in:
Randy Mackay 2019-01-24 11:55:47 +09:00
parent bdf6c09a76
commit 88e8a849bd
2 changed files with 93 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
APMrover2/AP_MotorsUGV.cpp
View File

@ -173,87 +173,6 @@ void AP_MotorsUGV::setup_servo_output()
SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100); SRV_Channels::set_range(SRV_Channel::k_mainsail_sheet, 100);
} }
// setup for frames with omni motors
void AP_MotorsUGV::setup_omni()
{
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
clear_omni_motors(i);
}
// hard coded factor configuration
switch (rover.get_frame_type()) {
// FRAME TYPE NAME
case FRAME_TYPE_UNDEFINED:
break;
case FRAME_TYPE_OMNI3:
_motors_num = 3;
add_omni_motor(0, 1.0f, 1.0f, -1.0f);
add_omni_motor(1, 0.0f, 1.0f, 1.0f);
add_omni_motor(2, 1.0f, 1.0f, 1.0f);
break;
case FRAME_TYPE_OMNIX:
_motors_num = 4,
add_omni_motor(0, 1.0f, -1.0f, -1.0f);
add_omni_motor(1, 1.0f, -1.0f, 1.0f);
add_omni_motor(2, 1.0f, 1.0f, -1.0f);
add_omni_motor(3, 1.0f, 1.0f, 1.0f);
break;
case FRAME_TYPE_OMNIPLUS:
_motors_num = 4;
add_omni_motor(0, 0.0f, 1.0f, 1.0f);
add_omni_motor(1, 1.0f, 0.0f, 0.0f);
add_omni_motor(2, 0.0f, -1.0f, 1.0f);
add_omni_motor(3, 1.0f, 0.0f, 0.0f);
break;
}
}
// add omni motor using separate throttle, steering and lateral factors
void AP_MotorsUGV::add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// set throttle, steering and lateral factors
_throttle_factor[motor_num] = throttle_factor;
_steering_factor[motor_num] = steering_factor;
_lateral_factor[motor_num] = lateral_factor;
add_omni_motor_num(motor_num);
}
}
// add an omni motor and set up default output function
void AP_MotorsUGV::add_omni_motor_num(int8_t motor_num)
{
// ensure a valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
uint8_t chan;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
SRV_Channels::set_aux_channel_default(function, motor_num);
if (!SRV_Channels::find_channel(function, chan)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
}
}
}
// disable omni motor and remove all throttle, steering and lateral factor for this motor
void AP_MotorsUGV::clear_omni_motors(int8_t motor_num)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// disable the motor and set factors to zero
_throttle_factor[motor_num] = 0;
_steering_factor[motor_num] = 0;
_lateral_factor[motor_num] = 0;
}
}
// set steering as a value from -4500 to +4500 // set steering as a value from -4500 to +4500
// apply_scaling should be set to false for manual modes where // apply_scaling should be set to false for manual modes where
// no scaling by speed or angle should be performed // no scaling by speed or angle should be performed
@ -568,6 +487,87 @@ void AP_MotorsUGV::setup_pwm_type()
} }
} }
// setup for frames with omni motors
void AP_MotorsUGV::setup_omni()
{
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_NUM_MOTORS_MAX; i++) {
clear_omni_motors(i);
}
// hard coded factor configuration
switch (rover.get_frame_type()) {
// FRAME TYPE NAME
case FRAME_TYPE_UNDEFINED:
break;
case FRAME_TYPE_OMNI3:
_motors_num = 3;
add_omni_motor(0, 1.0f, 1.0f, -1.0f);
add_omni_motor(1, 0.0f, 1.0f, 1.0f);
add_omni_motor(2, 1.0f, 1.0f, 1.0f);
break;
case FRAME_TYPE_OMNIX:
_motors_num = 4,
add_omni_motor(0, 1.0f, -1.0f, -1.0f);
add_omni_motor(1, 1.0f, -1.0f, 1.0f);
add_omni_motor(2, 1.0f, 1.0f, -1.0f);
add_omni_motor(3, 1.0f, 1.0f, 1.0f);
break;
case FRAME_TYPE_OMNIPLUS:
_motors_num = 4;
add_omni_motor(0, 0.0f, 1.0f, 1.0f);
add_omni_motor(1, 1.0f, 0.0f, 0.0f);
add_omni_motor(2, 0.0f, -1.0f, 1.0f);
add_omni_motor(3, 1.0f, 0.0f, 0.0f);
break;
}
}
// add omni motor using separate throttle, steering and lateral factors
void AP_MotorsUGV::add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// set throttle, steering and lateral factors
_throttle_factor[motor_num] = throttle_factor;
_steering_factor[motor_num] = steering_factor;
_lateral_factor[motor_num] = lateral_factor;
add_omni_motor_num(motor_num);
}
}
// add an omni motor and set up default output function
void AP_MotorsUGV::add_omni_motor_num(int8_t motor_num)
{
// ensure a valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
uint8_t chan;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
SRV_Channels::set_aux_channel_default(function, motor_num);
if (!SRV_Channels::find_channel(function, chan)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
}
}
}
// disable omni motor and remove all throttle, steering and lateral factor for this motor
void AP_MotorsUGV::clear_omni_motors(int8_t motor_num)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_NUM_MOTORS_MAX) {
// disable the motor and set factors to zero
_throttle_factor[motor_num] = 0;
_steering_factor[motor_num] = 0;
_lateral_factor[motor_num] = 0;
}
}
// output to regular steering and throttle channels // output to regular steering and throttle channels
void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle) void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering, float throttle)
{ {

24
APMrover2/AP_MotorsUGV.h
View File

@ -51,18 +51,6 @@ public:
// setup servo output ranges // setup servo output ranges
void setup_servo_output(); void setup_servo_output();
// setup for frames with omni motors
void setup_omni();
// add omni motor using separate throttle, steering and lateral factors
void add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor);
// add a motor and set up output function
void add_omni_motor_num(int8_t motor_num);
// disable omni motor and remove all throttle, steering and lateral factor for this motor
void clear_omni_motors(int8_t motor_num);
// get or set steering as a value from -4500 to +4500 // get or set steering as a value from -4500 to +4500
// apply_scaling should be set to false for manual modes where // apply_scaling should be set to false for manual modes where
// no scaling by speed or angle should e performed // no scaling by speed or angle should e performed
@ -128,6 +116,18 @@ protected:
// setup pwm output type // setup pwm output type
void setup_pwm_type(); void setup_pwm_type();
// setup for frames with omni motors
void setup_omni();
// add omni motor using separate throttle, steering and lateral factors
void add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor);
// add a motor and set up output function
void add_omni_motor_num(int8_t motor_num);
// disable omni motor and remove all throttle, steering and lateral factor for this motor
void clear_omni_motors(int8_t motor_num);
// output to regular steering and throttle channels // output to regular steering and throttle channels
void output_regular(bool armed, float ground_speed, float steering, float throttle); void output_regular(bool armed, float ground_speed, float steering, float throttle);