2012-04-30 04:17:14 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
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;
|
|
|
|
}
|
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
|
|
|
channel_throttle->set_radio_out (constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp));
|
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)) {
|
2013-03-21 21:54:04 -03:00
|
|
|
float xaccel = ins.get_accel().x;
|
|
|
|
if (xaccel >= g.auto_kickstart) {
|
2015-11-03 01:19:03 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (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) {
|
2014-02-16 19:08:59 -04:00
|
|
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
|
|
|
if (abs(bearing_error) > g.pivot_turn_angle) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
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
|
|
|
|
if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) {
|
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
|
|
|
channel_throttle->set_servo_out(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) {
|
|
|
|
channel_steer->set_servo_out(0);
|
|
|
|
}
|
2013-03-21 19:38:25 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2013-11-17 19:58:22 -04:00
|
|
|
float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
|
2015-11-04 23:34:24 -04:00
|
|
|
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
|
|
|
|
int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
|
|
|
|
float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
|
|
|
|
float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
|
|
|
|
|
2015-05-02 06:17:06 -03: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
|
2015-05-02 06:17:06 -03:00
|
|
|
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) {
|
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
|
|
|
channel_throttle->set_servo_out(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
|
2013-11-17 19:58:22 -04:00
|
|
|
} else {
|
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
|
|
|
channel_throttle->set_servo_out(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
|
|
|
|
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
|
2014-03-30 18:44:19 -03:00
|
|
|
int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
|
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
|
|
|
channel_throttle->set_servo_out(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()) {
|
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
|
|
|
channel_throttle->set_servo_out(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:
|
2014-03-17 04:44:25 -03:00
|
|
|
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()) {
|
|
|
|
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
|
|
|
|
if (bearing_error > 0) {
|
|
|
|
lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
|
|
|
|
} else {
|
|
|
|
lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
|
|
|
|
}
|
|
|
|
}
|
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
|
|
|
|
if ((loiter_time > 0) && (control_mode == AUTO)) {
|
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
|
|
|
channel_steer->set_servo_out(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
|
|
|
|
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
|
|
|
|
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
|
|
|
channel_steer->set_servo_out(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) {
|
2014-10-08 18:59:26 -03:00
|
|
|
static int16_t last_throttle;
|
2012-11-27 21:13:39 -04:00
|
|
|
|
2014-03-04 21:13:35 -04:00
|
|
|
// support a separate steering channel
|
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
|
|
|
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
|
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
|
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
|
|
|
channel_steer->set_radio_out(channel_steer->read());
|
|
|
|
channel_throttle->set_radio_out(channel_throttle->read());
|
2013-03-28 20:25:53 -03:00
|
|
|
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
|
|
|
|
// suppress throttle if in failsafe and manual
|
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
|
|
|
channel_throttle->set_radio_out(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) {
|
|
|
|
channel_steer->set_radio_out(channel_steer->get_radio_trim());
|
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
2015-11-04 23:34:24 -04:00
|
|
|
} else {
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_steer->calc_pwm();
|
2013-11-17 19:58:22 -04:00
|
|
|
if (in_reverse) {
|
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
|
|
|
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
|
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 {
|
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
|
|
|
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(),
|
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
|
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
|
|
|
channel_throttle->set_servo_out(0);
|
2016-05-25 05:17:03 -03:00
|
|
|
// suppress steer if in failsafe and skid steer mode
|
|
|
|
if (g.skid_steer_out) {
|
|
|
|
channel_steer->set_servo_out(0);
|
|
|
|
}
|
2013-03-28 20:25:53 -03:00
|
|
|
}
|
|
|
|
|
2015-10-30 02:56:41 -03:00
|
|
|
if (!hal.util->get_soft_armed()) {
|
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
|
|
|
channel_throttle->set_servo_out(0);
|
2016-05-25 05:17:03 -03:00
|
|
|
// suppress steer if in failsafe and skid steer mode
|
|
|
|
if (g.skid_steer_out) {
|
|
|
|
channel_steer->set_servo_out(0);
|
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
}
|
|
|
|
|
2012-11-27 06:47:30 -04:00
|
|
|
// convert 0 to 100% into PWM
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->calc_pwm();
|
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
|
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
|
|
|
last_throttle = channel_throttle->get_radio_out();
|
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
|
|
|
|
*/
|
2014-10-08 18:59:26 -03:00
|
|
|
float steering_scaled = channel_steer->norm_output();
|
|
|
|
float throttle_scaled = channel_throttle->norm_output();
|
2015-05-02 06:17:06 -03:00
|
|
|
float motor1 = throttle_scaled + 0.5f*steering_scaled;
|
|
|
|
float motor2 = throttle_scaled - 0.5f*steering_scaled;
|
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
|
|
|
channel_steer->set_servo_out(4500*motor1);
|
|
|
|
channel_throttle->set_servo_out(100*motor2);
|
2014-10-08 18:59:26 -03:00
|
|
|
channel_steer->calc_pwm();
|
|
|
|
channel_throttle->calc_pwm();
|
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()) {
|
|
|
|
//Some ESCs get noisy (beep error msgs) if PWM == 0.
|
|
|
|
//This little segment aims to avoid this.
|
|
|
|
switch (arming.arming_required()) {
|
|
|
|
case AP_Arming::NO:
|
|
|
|
//keep existing behavior: do nothing to radio_out
|
|
|
|
//(don't disarm throttle channel even if AP_Arming class is)
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_ZERO_PWM:
|
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
|
|
|
channel_throttle->set_radio_out(0);
|
2016-05-25 05:17:03 -03:00
|
|
|
if (g.skid_steer_out) {
|
|
|
|
channel_steer->set_radio_out(0);
|
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_Arming::YES_MIN_PWM:
|
|
|
|
default:
|
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
|
|
|
channel_throttle->set_radio_out(channel_throttle->get_radio_trim());
|
2016-05-25 05:17:03 -03:00
|
|
|
if (g.skid_steer_out) {
|
|
|
|
channel_steer->set_radio_out(channel_steer->get_radio_trim());
|
|
|
|
}
|
2015-10-30 02:56:41 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
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
|
|
|
|
// ----------------------------------------
|
|
|
|
channel_steer->output();
|
2013-06-03 06:33:59 -03:00
|
|
|
channel_throttle->output();
|
2014-04-02 22:19:25 -03:00
|
|
|
RC_Channel_aux::output_ch_all();
|
2012-04-30 04:17:14 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2013-09-01 20:46:04 -03:00
|
|
|
|