2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
|
|
|
|
2012-11-27 20:42:22 -04:00
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Throttle slew limit
|
2012-11-27 20:42:22 -04:00
|
|
|
*****************************************/
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::throttle_slew_limit(int16_t last_throttle) {
|
2012-11-27 20:42:22 -04:00
|
|
|
// if slew limit rate is set to zero then do not slew limit
|
2015-11-04 23:34:24 -04:00
|
|
|
if (g.throttle_slewrate && last_throttle != 0) {
|
2012-11-27 20:42:22 -04:00
|
|
|
// limit throttle change by the given percentage per second
|
APMRover2: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:49:39 -03:00
|
|
|
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->get_radio_max() - channel_throttle->get_radio_min());
|
2012-11-27 21:13:39 -04:00
|
|
|
// allow a minimum change of 1 PWM per cycle
|
|
|
|
if (temp < 1) {
|
|
|
|
temp = 1;
|
|
|
|
}
|
2017-01-06 06:31:10 -04:00
|
|
|
uint16_t pwm;
|
|
|
|
if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, pwm)) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, constrain_int16(pwm, last_throttle - temp, last_throttle + temp));
|
2017-01-06 06:31:10 -04:00
|
|
|
}
|
2012-11-27 20:42:22 -04:00
|
|
|
}
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
check for triggering of start of auto mode
|
|
|
|
*/
|
|
|
|
bool Rover::auto_check_trigger(void) {
|
2013-03-21 19:38:25 -03:00
|
|
|
// only applies to AUTO mode
|
|
|
|
if (control_mode != AUTO) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2013-05-02 20:19:20 -03:00
|
|
|
// check for user pressing the auto trigger to off
|
|
|
|
if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
|
2013-05-02 20:19:20 -03:00
|
|
|
auto_triggered = false;
|
2015-11-04 23:34:24 -04:00
|
|
|
return false;
|
2013-05-02 20:19:20 -03:00
|
|
|
}
|
|
|
|
|
2013-03-21 19:38:25 -03:00
|
|
|
// if already triggered, then return true, so you don't
|
|
|
|
// need to hold the switch down
|
|
|
|
if (auto_triggered) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-05-04 23:34:02 -03:00
|
|
|
if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) {
|
2013-03-21 19:38:25 -03:00
|
|
|
// no trigger configured - let's go!
|
|
|
|
auto_triggered = true;
|
|
|
|
return true;
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-05-02 20:19:20 -03:00
|
|
|
if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
|
2015-10-24 19:36:35 -03:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
|
2013-05-02 20:19:20 -03:00
|
|
|
auto_triggered = true;
|
2015-11-04 23:34:24 -04:00
|
|
|
return true;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
|
2015-05-04 23:34:02 -03:00
|
|
|
if (!is_zero(g.auto_kickstart)) {
|
2017-01-31 06:12:26 -04:00
|
|
|
const float xaccel = ins.get_accel().x;
|
2013-03-21 21:54:04 -03:00
|
|
|
if (xaccel >= g.auto_kickstart) {
|
2017-03-30 11:07:24 -03:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
|
2013-03-21 19:38:25 -03:00
|
|
|
auto_triggered = true;
|
2015-11-04 23:34:24 -04:00
|
|
|
return true;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
return false;
|
2013-03-21 19:38:25 -03:00
|
|
|
}
|
|
|
|
|
2014-02-16 19:08:59 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
work out if we are going to use pivot steering
|
|
|
|
*/
|
|
|
|
bool Rover::use_pivot_steering(void) {
|
2014-02-23 17:23:20 -04:00
|
|
|
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
|
2017-01-31 06:12:26 -04:00
|
|
|
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
2014-02-16 19:08:59 -04:00
|
|
|
if (abs(bearing_error) > g.pivot_turn_angle) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-10-30 07:42:46 -03:00
|
|
|
/*
|
|
|
|
test if we are loitering AND should be stopped at a waypoint
|
|
|
|
*/
|
|
|
|
bool Rover::in_stationary_loiter()
|
|
|
|
{
|
|
|
|
// Confirm we are in AUTO mode and need to loiter for a time period
|
|
|
|
if ((loiter_start_time > 0) && (control_mode == AUTO)) {
|
|
|
|
// Check if active loiter is enabled AND we are outside the waypoint loiter radius
|
2016-12-19 23:31:42 -04:00
|
|
|
// then the vehicle still needs to move so return false
|
|
|
|
if (active_loiter && (wp_distance > g.waypoint_radius)) {
|
|
|
|
return false;
|
2016-10-30 07:42:46 -03:00
|
|
|
}
|
2016-12-19 23:31:42 -04:00
|
|
|
return true;
|
2016-10-30 07:42:46 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
2013-03-21 19:38:25 -03:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
calculate the throtte for auto-throttle modes
|
|
|
|
*/
|
|
|
|
void Rover::calc_throttle(float target_speed) {
|
2015-07-27 09:10:37 -03:00
|
|
|
// If not autostarting OR we are loitering at a waypoint
|
|
|
|
// then set the throttle to minimum
|
2016-10-30 07:42:46 -03:00
|
|
|
if (!auto_check_trigger() || in_stationary_loiter()) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
|
2016-05-25 05:17:03 -03:00
|
|
|
// Stop rotation in case of loitering and skid steering
|
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2013-03-21 19:38:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-01-31 06:12:26 -04:00
|
|
|
const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
|
|
|
|
const int throttle_target = throttle_base + throttle_nudge;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
reduce target speed in proportion to turning rate, up to the
|
|
|
|
SPEED_TURN_GAIN percentage.
|
2013-03-01 07:32:57 -04:00
|
|
|
*/
|
2013-06-16 20:50:53 -03:00
|
|
|
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
|
2015-05-02 06:17:06 -03:00
|
|
|
steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
|
2014-04-06 20:30:39 -03:00
|
|
|
|
|
|
|
// use g.speed_turn_gain for a 90 degree turn, and in proportion
|
|
|
|
// for other turn angles
|
2017-01-31 06:12:26 -04:00
|
|
|
const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
|
|
|
|
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
|
|
|
|
const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
|
2014-04-06 20:30:39 -03:00
|
|
|
|
2017-01-16 05:18:55 -04:00
|
|
|
float reduction = 1.0f - steer_rate * speed_turn_reduction;
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
|
|
|
|
// in auto-modes we reduce speed when approaching waypoints
|
2017-01-31 06:12:26 -04:00
|
|
|
const float reduction2 = 1.0f - speed_turn_reduction;
|
2013-03-01 20:03:15 -04:00
|
|
|
if (reduction2 < reduction) {
|
|
|
|
reduction = reduction2;
|
|
|
|
}
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
// reduce the target speed by the reduction factor
|
|
|
|
target_speed *= reduction;
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
groundspeed_error = fabsf(target_speed) - ground_speed;
|
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-03-01 20:03:15 -04:00
|
|
|
// also reduce the throttle by the reduction factor. This gives a
|
|
|
|
// much faster response in turns
|
|
|
|
throttle *= reduction;
|
2013-03-01 07:32:57 -04:00
|
|
|
|
2013-11-17 19:58:22 -04:00
|
|
|
if (in_reverse) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
|
2013-11-17 19:58:22 -04:00
|
|
|
} else {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max));
|
2013-11-17 19:58:22 -04:00
|
|
|
}
|
2014-03-30 18:44:19 -03:00
|
|
|
|
2014-04-06 19:42:54 -03:00
|
|
|
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
|
2014-03-30 18:44:19 -03:00
|
|
|
// the user has asked to use reverse throttle to brake. Apply
|
|
|
|
// it in proportion to the ground speed error, but only when
|
2014-04-06 19:42:54 -03:00
|
|
|
// our ground speed error is more than BRAKING_SPEEDERR.
|
|
|
|
//
|
|
|
|
// We use a linear gain, with 0 gain at a ground speed error
|
|
|
|
// of braking_speederr, and 100% gain when groundspeed_error
|
|
|
|
// is 2*braking_speederr
|
2017-01-31 06:12:26 -04:00
|
|
|
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
|
|
|
|
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
|
2014-10-08 18:59:26 -03:00
|
|
|
|
2014-03-30 18:44:19 -03:00
|
|
|
// temporarily set us in reverse to allow the PWM setting to
|
|
|
|
// go negative
|
|
|
|
set_reverse(true);
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
|
2014-02-16 19:08:59 -04:00
|
|
|
if (use_pivot_steering()) {
|
2017-01-31 05:46:32 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
|
2014-02-16 19:08:59 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Calculate desired turn angles (in medium freq loop)
|
2012-04-30 04:17:14 -03:00
|
|
|
*****************************************/
|
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::calc_lateral_acceleration() {
|
2013-06-16 20:50:53 -03:00
|
|
|
switch (control_mode) {
|
|
|
|
case AUTO:
|
2016-10-30 07:42:46 -03:00
|
|
|
// If we have reached the waypoint previously navigate
|
|
|
|
// back to it from our current position
|
|
|
|
if (previously_reached_wp && (loiter_duration > 0)) {
|
|
|
|
nav_controller->update_waypoint(current_loc, next_WP);
|
|
|
|
} else {
|
|
|
|
nav_controller->update_waypoint(prev_WP, next_WP);
|
|
|
|
}
|
2013-06-16 20:50:53 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case RTL:
|
|
|
|
case GUIDED:
|
|
|
|
case STEERING:
|
2014-03-17 04:44:25 -03:00
|
|
|
nav_controller->update_waypoint(current_loc, next_WP);
|
2013-06-16 20:50:53 -03:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
return;
|
2013-03-28 19:14:58 -03:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
// Calculate the required turn of the wheels
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2012-11-29 03:09:05 -04:00
|
|
|
// negative error = left turn
|
2015-11-04 23:34:24 -04:00
|
|
|
// positive error = right turn
|
2013-06-16 20:50:53 -03:00
|
|
|
lateral_acceleration = nav_controller->lateral_acceleration();
|
2014-02-16 19:08:59 -04:00
|
|
|
if (use_pivot_steering()) {
|
2017-01-31 06:12:26 -04:00
|
|
|
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
2014-02-16 19:08:59 -04:00
|
|
|
if (bearing_error > 0) {
|
2017-01-16 05:18:55 -04:00
|
|
|
lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
|
2014-02-16 19:08:59 -04:00
|
|
|
} else {
|
2017-01-16 05:18:55 -04:00
|
|
|
lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;
|
2014-02-16 19:08:59 -04:00
|
|
|
}
|
|
|
|
}
|
2013-06-16 20:50:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
calculate steering angle given lateral_acceleration
|
|
|
|
*/
|
|
|
|
void Rover::calc_nav_steer() {
|
2015-07-27 09:10:37 -03:00
|
|
|
// check to see if the rover is loitering
|
2016-10-30 07:42:46 -03:00
|
|
|
if (in_stationary_loiter()) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
|
2015-07-27 09:10:37 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2013-06-16 20:50:53 -03:00
|
|
|
// add in obstacle avoidance
|
2016-07-14 04:50:39 -03:00
|
|
|
if (!in_reverse) {
|
|
|
|
lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
|
|
|
|
}
|
2013-06-16 20:50:53 -03:00
|
|
|
|
|
|
|
// constrain to max G force
|
2017-01-16 05:18:55 -04:00
|
|
|
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
|
2012-06-13 15:43:35 -03:00
|
|
|
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steerController.get_steering_out_lat_accel(lateral_acceleration));
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*****************************************
|
2015-11-04 23:34:24 -04:00
|
|
|
Set the flight control servos based on the current calculated values
|
2012-04-30 04:17:14 -03:00
|
|
|
*****************************************/
|
2015-11-04 23:34:24 -04:00
|
|
|
void Rover::set_servos(void) {
|
2017-01-06 06:31:10 -04:00
|
|
|
static uint16_t last_throttle;
|
2017-01-16 05:19:28 -04:00
|
|
|
bool apply_skid_mix = true; // Normaly true, false when the mixage is done by the controler with skid_steer_in = 1
|
2014-03-04 21:13:35 -04:00
|
|
|
|
2015-11-04 23:34:24 -04:00
|
|
|
if (control_mode == MANUAL || control_mode == LEARNING) {
|
2013-03-14 21:04:33 -03:00
|
|
|
// do a direct pass through of radio values
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read());
|
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read());
|
2013-03-28 20:25:53 -03:00
|
|
|
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
|
|
|
|
// suppress throttle if in failsafe and manual
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
|
2016-05-25 05:17:03 -03:00
|
|
|
// suppress steer if in failsafe and manual and skid steer mode
|
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
2017-01-16 05:19:28 -04:00
|
|
|
if (g.skid_steer_in) {
|
|
|
|
apply_skid_mix = false;
|
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
} else {
|
2013-11-17 19:58:22 -04:00
|
|
|
if (in_reverse) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
|
2015-11-04 23:34:24 -04:00
|
|
|
-g.throttle_max,
|
APMRover2: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:49:39 -03:00
|
|
|
-g.throttle_min));
|
2013-11-17 19:58:22 -04:00
|
|
|
} else {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
|
2015-11-04 23:34:24 -04:00
|
|
|
g.throttle_min.get(),
|
APMRover2: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:49:39 -03:00
|
|
|
g.throttle_max.get()));
|
2013-11-17 19:58:22 -04:00
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
|
|
|
|
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
|
|
|
|
// suppress throttle if in failsafe
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
// suppress steer if in failsafe and skid steer mode
|
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
if (!hal.util->get_soft_armed()) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
// suppress steer if in failsafe and skid steer mode
|
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
}
|
|
|
|
|
2012-11-27 21:13:39 -04:00
|
|
|
// limit throttle movement speed
|
|
|
|
throttle_slew_limit(last_throttle);
|
2014-10-08 18:59:26 -03:00
|
|
|
}
|
2013-03-14 21:04:33 -03:00
|
|
|
|
2014-10-08 18:59:26 -03:00
|
|
|
// record last throttle before we apply skid steering
|
2017-01-06 06:31:10 -04:00
|
|
|
SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle);
|
2014-10-08 18:59:26 -03:00
|
|
|
|
|
|
|
if (g.skid_steer_out) {
|
|
|
|
// convert the two radio_out values to skid steering values
|
|
|
|
/*
|
2015-11-04 23:34:24 -04:00
|
|
|
mixing rule:
|
|
|
|
steering = motor1 - motor2
|
|
|
|
throttle = 0.5*(motor1 + motor2)
|
|
|
|
motor1 = throttle + 0.5*steering
|
|
|
|
motor2 = throttle - 0.5*steering
|
|
|
|
*/
|
2017-01-16 05:18:55 -04:00
|
|
|
const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
|
|
|
|
const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
|
|
|
|
float motor1 = throttle_scaled + 0.5f * steering_scaled;
|
|
|
|
float motor2 = throttle_scaled - 0.5f * steering_scaled;
|
2017-01-16 05:19:28 -04:00
|
|
|
|
|
|
|
if (!apply_skid_mix) { // Mixage is already done by a controller so just pass the value to motor
|
|
|
|
motor1 = steering_scaled;
|
|
|
|
motor2 = throttle_scaled;
|
|
|
|
}
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
|
2012-11-27 06:47:30 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
if (!arming.is_armed()) {
|
2016-12-20 09:31:25 -04:00
|
|
|
// Some ESCs get noisy (beep error msgs) if PWM == 0.
|
|
|
|
// This little segment aims to avoid this.
|
2015-10-30 02:56:41 -03:00
|
|
|
switch (arming.arming_required()) {
|
|
|
|
case AP_Arming::NO:
|
2016-12-20 09:31:25 -04:00
|
|
|
// keep existing behavior: do nothing to radio_out
|
|
|
|
// (don't disarm throttle channel even if AP_Arming class is)
|
2015-10-30 02:56:41 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_ZERO_PWM:
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0);
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_MIN_PWM:
|
|
|
|
default:
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim());
|
2016-05-25 05:17:03 -03:00
|
|
|
if (g.skid_steer_out) {
|
2017-01-16 05:18:55 -04:00
|
|
|
SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim());
|
2016-05-25 05:17:03 -03:00
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2017-01-06 06:31:10 -04:00
|
|
|
SRV_Channels::calc_pwm();
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
2015-11-04 23:34:24 -04:00
|
|
|
// send values to the PWM timers for output
|
|
|
|
// ----------------------------------------
|
2017-01-06 06:31:10 -04:00
|
|
|
SRV_Channels::output_ch_all();
|
2012-04-30 04:17:14 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2013-09-01 20:46:04 -03:00
|
|
|
|