From 430ed9bd3db1c851796df977c5214d7c4b3a56d0 Mon Sep 17 00:00:00 2001 From: Ammarf <35584084+Ammarf@users.noreply.github.com> Date: Thu, 31 May 2018 18:26:07 +0900 Subject: [PATCH] Rover: add custom config support --- APMrover2/AP_MotorsUGV.cpp | 248 ++++++++++++++++++++++++------------- APMrover2/AP_MotorsUGV.h | 39 ++++-- APMrover2/Parameters.cpp | 8 ++ APMrover2/Parameters.h | 3 + APMrover2/Rover.h | 3 + 5 files changed, 208 insertions(+), 93 deletions(-) diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index b1d78cca4d..ca4f8b9b7b 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -115,6 +115,11 @@ void AP_MotorsUGV::init() // set safety output setup_safety_output(); + + // setup motors for custom configs + if (rover.get_frame_type() != FRAME_TYPE_UNDEFINED) { + setup_motors(); + } } // setup output in case of main CPU failure @@ -158,10 +163,92 @@ void AP_MotorsUGV::setup_servo_output() SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000); SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000); - // k_motor1, k_motor2 and k_motor3 are in power percent so -100 ... 100 - SRV_Channels::set_angle(SRV_Channel::k_motor1, 100); - SRV_Channels::set_angle(SRV_Channel::k_motor2, 100); - SRV_Channels::set_angle(SRV_Channel::k_motor3, 100); + // custom config motors set in power percent so -100 ... 100 + for (uint8_t i=0; i= 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_motor_num(motor_num); + } +} + +// add a motor and set up default output function +void AP_MotorsUGV::add_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 motor and remove all throttle, steering and lateral factor for this motor +void AP_MotorsUGV::clear_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 @@ -216,17 +303,6 @@ bool AP_MotorsUGV::have_skid_steering() const return false; } -// returns true if vehicle is capable of lateral movement -bool AP_MotorsUGV::has_lateral_control() 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 ground_speed, float dt) { // soft-armed overrides passed in armed status @@ -244,12 +320,12 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float 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, _lateral); - // output for skid steering style frames output_skid_steering(armed, _steering, _throttle); + // output for frames with vectored and custom motor configurations + output_custom_config(armed, _steering, _throttle, _lateral); + // send values to the PWM timers for output SRV_Channels::calc_pwm(); SRV_Channels::cork(); @@ -379,12 +455,17 @@ 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"); + // check if one of custom config motors hasn't been configured + for (uint8_t i=0; i<_motors_num; i++) + { + SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i); + if (SRV_Channels::function_assigned(function)) { + return true; + } else { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check %u", function); + return false; + } } } return true; @@ -407,9 +488,9 @@ void AP_MotorsUGV::setup_pwm_type() motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttle); motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleLeft); motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_throttleRight); - motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor1); - motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor2); - motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channel::k_motor3); + for (uint8_t i=0; i<_motors_num; i++) { + motor_mask |= SRV_Channels::get_output_channel_mask(SRV_Channels::get_motor_function(i)); + } switch (_pwm_type) { case PWM_TYPE_ONESHOT: @@ -484,64 +565,6 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float 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, 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) { @@ -591,11 +614,60 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott 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 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 throttle value to main throttle channel, left throttle or right throttle. throttle should be scaled from -100 to 100 void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle) { // sanity check servo function - if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3) { + if (function != SRV_Channel::k_throttle && function != SRV_Channel::k_throttleLeft && function != SRV_Channel::k_throttleRight && function != SRV_Channel::k_motor1 && function != SRV_Channel::k_motor2 && function != SRV_Channel::k_motor3 && function!= SRV_Channel::k_motor4) { return; } @@ -625,6 +697,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f case SRV_Channel::k_motor3: _relayEvents.do_set_relay(2, relay_high); break; + case SRV_Channel::k_motor4: + _relayEvents.do_set_relay(3, relay_high); + break; default: // do nothing break; @@ -639,6 +714,7 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f case SRV_Channel::k_motor1: case SRV_Channel::k_motor2: case SRV_Channel::k_motor3: + case SRV_Channel::k_motor4: SRV_Channels::set_output_scaled(function, throttle); break; case SRV_Channel::k_throttleLeft: diff --git a/APMrover2/AP_MotorsUGV.h b/APMrover2/AP_MotorsUGV.h index 1a532f0775..91baea86ac 100644 --- a/APMrover2/AP_MotorsUGV.h +++ b/APMrover2/AP_MotorsUGV.h @@ -25,6 +25,14 @@ public: MOTOR_TEST_THROTTLE_RIGHT = 4, }; + // supported custom motor configurations + enum frame_type { + FRAME_TYPE_UNDEFINED = 0, + FRAME_TYPE_OMNI3 = 1, + FRAME_TYPE_OMNIX = 2, + FRAME_TYPE_OMNIPLUS = 3, + }; + // initialise motors void init(); @@ -37,6 +45,18 @@ public: // setup servo output ranges void setup_servo_output(); + // config for frames with vectored motors + void setup_motors(); + + // add motor using separate throttle, steering and lateral factors for frames with custom motor configuration + void add_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor); + + // add a motor and set up output function + void add_motor_num(int8_t motor_num); + + // disable motor and remove all throttle, steering and lateral factor for this motor + void clear_motors(int8_t motor_num); + // get or set steering as a value from -4500 to +4500 // apply_scaling should be set to false for manual modes where // no scaling by speed or angle should e performed @@ -58,9 +78,6 @@ public: // true if vehicle is capable of skid steering bool have_skid_steering() const; - //true if vehicle is capable of lateral movement - bool has_lateral_control() 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); } @@ -101,12 +118,12 @@ protected: // output to regular steering and throttle channels void output_regular(bool armed, float ground_speed, float steering, float throttle); - // output for omni style frames - void output_omni(bool armed, float steering, float throttle, float lateral); - // output to skid steering channels void output_skid_steering(bool armed, float steering, float throttle); + // output for vectored and custom motors configuration + void output_custom_config(bool armed, float steering, float throttle, float lateral); + // output throttle (-100 ~ +100) to a throttle channel. Sets relays if required void output_throttle(SRV_Channel::Aux_servo_function_t function, float throttle); @@ -122,6 +139,8 @@ protected: // external references AP_ServoRelayEvents &_relayEvents; + static const int8_t AP_MOTORS_NUM_MOTORS_MAX = 4; + // parameters AP_Int8 _pwm_type; // PWM output type AP_Int8 _pwm_freq; // PWM output freq for brushed motors @@ -138,5 +157,11 @@ protected: float _throttle; // requested throttle as a value from -100 to 100 float _throttle_prev; // throttle input from previous iteration bool _scale_steering = true; // true if we should scale steering by speed or angle - float _lateral; // requested lateral input as a value from -4500 to +4500 + float _lateral; // requested lateral input as a value from -100 to +100 + + // custom config variables + float _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX]; + float _steering_factor[AP_MOTORS_NUM_MOTORS_MAX]; + float _lateral_factor[AP_MOTORS_NUM_MOTORS_MAX]; + uint8_t _motors_num; }; diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index f4135c9a36..f2847a5d16 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -556,6 +556,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 23, ParametersG2, AP_Follow), + // @Param: FRAME_TYPE + // @DisplayName: Frame Type + // @Description: Frame Type + // @Values: 0:Undefined,1:Omni3,2:OmniX,3:OmniPlus + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("FRAME_TYPE", 24, ParametersG2, frame_type, 0), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 4ffae11446..6413415b56 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -351,6 +351,9 @@ public: // follow mode library AP_Follow follow; + + // frame type for vehicle (used for vectored motor vehicles and custom motor configs) + AP_Int8 frame_type; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6b8786f76f..100ff80b9d 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -563,6 +563,9 @@ public: bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, int16_t throttle_value, float timeout_sec); void motor_test_stop(); + + // frame type + uint8_t get_frame_type() { return g2.frame_type.get(); } }; extern const AP_HAL::HAL& hal;