Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: convert to new SRV_Channel API

This commit is contained in:
Andrew Tridgell 2017-01-06 21:31:10 +11:00
parent 5a87ae3f01
commit 93d6b012c2
14 changed files with 112 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
APMrover2/APMrover2.cpp
View File

@ -300,7 +300,7 @@ void Rover::update_logging2(void)
*/ */
void Rover::update_aux(void) void Rover::update_aux(void)
{ {
RC_Channel_aux::enable_aux_servos(); SRV_Channels::enable_aux_servos();
} }
/* /*
@ -452,17 +452,18 @@ void Rover::update_current_mode(void)
case Guided_WP: case Guided_WP:
if (rtl_complete || verify_RTL()) { if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are // we have reached destination so stop where we are
if (channel_throttle->get_servo_out() != g.throttle_min.get()) { if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0); gcs_send_mission_item_reached_message(0);
} }
channel_throttle->set_servo_out(g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0; lateral_acceleration = 0;
} else { } else {
calc_lateral_acceleration(); calc_lateral_acceleration();
calc_nav_steer(); calc_nav_steer();
calc_throttle(g.speed_cruise); calc_throttle(g.speed_cruise);
Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt), Vector3f(g.speed_cruise, channel_throttle->get_servo_out(), 0)); Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt),
Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
} }
break; break;
@ -510,18 +511,18 @@ void Rover::update_current_mode(void)
we set the exact value in set_servos(), but it helps for we set the exact value in set_servos(), but it helps for
logging logging
*/ */
channel_throttle->set_servo_out(channel_throttle->get_control_in()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
channel_steer->set_servo_out(channel_steer->pwm_to_angle()); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,channel_steer->pwm_to_angle());
// mark us as in_reverse when using a negative throttle to // mark us as in_reverse when using a negative throttle to
// stop AHRS getting off // stop AHRS getting off
set_reverse(channel_throttle->get_servo_out() < 0); set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0);
break; break;
case HOLD: case HOLD:
// hold position - stop motors and center steering // hold position - stop motors and center steering
channel_throttle->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
break; break;
case INITIALISING: case INITIALISING:
@ -548,7 +549,7 @@ void Rover::update_navigation()
calc_lateral_acceleration(); calc_lateral_acceleration();
calc_nav_steer(); calc_nav_steer();
if (verify_RTL()) { if (verify_RTL()) {
channel_throttle->set_servo_out(g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
set_mode(HOLD); set_mode(HOLD);
} }
break; break;
@ -565,8 +566,8 @@ void Rover::update_navigation()
calc_nav_steer(); calc_nav_steer();
if (rtl_complete || verify_RTL()) { if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are // we have reached destination so stop where we are
channel_throttle->set_servo_out(g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0; lateral_acceleration = 0;
} }
break; break;

4
APMrover2/GCS_Mavlink.cpp
View File

@ -162,7 +162,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
0, // port 0 0, // port 0
10000 * channel_steer->norm_output(), 10000 * channel_steer->norm_output(),
0, 0,
10000 * channel_throttle->norm_output(), 10000 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle),
0, 0,
0, 0,
0, 0,
@ -179,7 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
gps.ground_speed(), gps.ground_speed(),
ahrs.groundspeed(), ahrs.groundspeed(),
(ahrs.yaw_sensor / 100) % 360, (ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * fabsf(channel_throttle->norm_output())), (uint16_t)(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))),
current_loc.alt / 100.0, current_loc.alt / 100.0,
0); 0);
} }

8
APMrover2/Log.cpp
View File

@ -238,10 +238,10 @@ void Rover::Log_Write_Control_Tuning()
struct log_Control_Tuning pkt = { struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
steer_out : (int16_t)channel_steer->get_servo_out(), steer_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_steering),
roll : (int16_t)ahrs.roll_sensor, roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor, pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)channel_throttle->get_servo_out(), throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
accel_y : accel.y accel_y : accel.y
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -268,7 +268,7 @@ void Rover::Log_Write_Nav_Tuning()
wp_distance : wp_distance, wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
throttle : (int8_t)(100 * channel_throttle->norm_output()), throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)),
xtrack_error : nav_controller->crosstrack_error() xtrack_error : nav_controller->crosstrack_error()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -326,7 +326,7 @@ void Rover::Log_Write_Sonar()
turn_angle : (int8_t)obstacle.turn_angle, turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time, turn_time : turn_time,
ground_speed : (uint16_t)(ground_speed*100), ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * channel_throttle->norm_output()) throttle : (int8_t)(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }

69
APMrover2/Parameters.cpp
View File

@ -174,62 +174,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard // @User: Standard
GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION), GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION),
// @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_1, "RC1_", RC_Channel),
// @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_2, "RC2_", RC_Channel_aux),
// @Group: RC3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_3, "RC3_", RC_Channel),
// @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_4, "RC4_", RC_Channel_aux),
// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),
// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),
// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),
// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),
// @Group: RC10_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_10, "RC10_", RC_Channel_aux),
// @Group: RC11_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_11, "RC11_", RC_Channel_aux),
// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),
// @Group: RC13_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_13, "RC13_", RC_Channel_aux),
// @Group: RC14_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),
// @Param: THR_MIN // @Param: THR_MIN
// @DisplayName: Minimum Throttle // @DisplayName: Minimum Throttle
// @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode. // @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode.
@ -585,10 +529,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("SYSID_ENFORCE", 2, ParametersG2, sysid_enforce, 0), AP_GROUPINFO("SYSID_ENFORCE", 2, ParametersG2, sysid_enforce, 0),
// @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels),
// @Group: RC
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels),
AP_GROUPEND AP_GROUPEND
}; };
ParametersG2::ParametersG2(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
/* /*
This is a conversion table from old parameter values to new This is a conversion table from old parameter values to new

68
APMrover2/Parameters.h
View File

@ -33,8 +33,8 @@ public:
k_param_relay, k_param_relay,
k_param_BoardConfig, k_param_BoardConfig,
k_param_pivot_turn_angle, k_param_pivot_turn_angle,
k_param_rc_13, k_param_rc_13_old,
k_param_rc_14, k_param_rc_14_old,
// IO pins // IO pins
k_param_rssi_pin = 20, // unused, replaced by rssi_ library parameters k_param_rssi_pin = 20, // unused, replaced by rssi_ library parameters
@ -113,14 +113,14 @@ public:
// //
// 160: Radio settings // 160: Radio settings
// //
k_param_rc_1 = 160, k_param_rc_1_old = 160,
k_param_rc_2, k_param_rc_2_old,
k_param_rc_3, k_param_rc_3_old,
k_param_rc_4, k_param_rc_4_old,
k_param_rc_5, k_param_rc_5_old,
k_param_rc_6, k_param_rc_6_old,
k_param_rc_7, k_param_rc_7_old,
k_param_rc_8, k_param_rc_8_old,
// throttle control // throttle control
k_param_throttle_min = 170, k_param_throttle_min = 170,
@ -182,10 +182,10 @@ public:
k_param_pidSpeedThrottle, k_param_pidSpeedThrottle,
// high RC channels // high RC channels
k_param_rc_9 = 235, k_param_rc_9_old = 235,
k_param_rc_10, k_param_rc_10_old,
k_param_rc_11, k_param_rc_11_old,
k_param_rc_12, k_param_rc_12_old,
// other objects // other objects
k_param_sitl = 240, k_param_sitl = 240,
@ -242,22 +242,6 @@ public:
AP_Int16 pivot_turn_angle; AP_Int16 pivot_turn_angle;
AP_Int16 gcs_pid_mask; AP_Int16 gcs_pid_mask;
// RC channels
RC_Channel rc_1;
RC_Channel_aux rc_2;
RC_Channel rc_3;
RC_Channel_aux rc_4;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
RC_Channel_aux rc_9;
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
RC_Channel_aux rc_12;
RC_Channel_aux rc_13;
RC_Channel_aux rc_14;
// Throttle // Throttle
// //
AP_Int8 throttle_min; AP_Int8 throttle_min;
@ -302,22 +286,6 @@ public:
PID pidSpeedThrottle; PID pidSpeedThrottle;
Parameters() : Parameters() :
// RC channels
rc_1(CH_1),
rc_2(CH_2),
rc_3(CH_3),
rc_4(CH_4),
rc_5(CH_5),
rc_6(CH_6),
rc_7(CH_7),
rc_8(CH_8),
rc_9(CH_9),
rc_10(CH_10),
rc_11(CH_11),
rc_12(CH_12),
rc_13(CH_13),
rc_14(CH_14),
// PID controller initial P initial I initial D initial imax // PID controller initial P initial I initial D initial imax
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
pidSpeedThrottle (0.7, 0.2, 0.2, 4000) pidSpeedThrottle (0.7, 0.2, 0.2, 4000)
@ -329,7 +297,7 @@ public:
*/ */
class ParametersG2 { class ParametersG2 {
public: public:
ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); } ParametersG2(void);
// var_info for holding Parameter information // var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -339,6 +307,12 @@ public:
// whether to enforce acceptance of packets only from sysid_my_gcs // whether to enforce acceptance of packets only from sysid_my_gcs
AP_Int8 sysid_enforce; AP_Int8 sysid_enforce;
// RC input channels
RC_Channels rc_channels;
// control over servo output ranges
SRV_Channels servo_channels;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

1
APMrover2/Rover.h
View File

@ -92,6 +92,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
public: public:
friend class GCS_MAVLINK_Rover; friend class GCS_MAVLINK_Rover;
friend class Parameters; friend class Parameters;
friend class ParametersG2;
friend class AP_Arming; friend class AP_Arming;
Rover(void); Rover(void);

76
APMrover2/Steering.cpp
View File

@ -12,7 +12,10 @@ void Rover::throttle_slew_limit(int16_t last_throttle) {
if (temp < 1) { if (temp < 1) {
temp = 1; temp = 1;
} }
channel_throttle->set_radio_out(constrain_int16(channel_throttle->get_radio_out(), last_throttle - temp, last_throttle + temp)); uint16_t pwm;
if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, pwm)) {
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,constrain_int16(pwm, last_throttle - temp, last_throttle + temp));
}
} }
} }
@ -100,10 +103,10 @@ void Rover::calc_throttle(float target_speed) {
// If not autostarting OR we are loitering at a waypoint // If not autostarting OR we are loitering at a waypoint
// then set the throttle to minimum // then set the throttle to minimum
if (!auto_check_trigger() || in_stationary_loiter()) { if (!auto_check_trigger() || in_stationary_loiter()) {
channel_throttle->set_servo_out(g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
// Stop rotation in case of loitering and skid steering // Stop rotation in case of loitering and skid steering
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
} }
return; return;
} }
@ -146,9 +149,9 @@ void Rover::calc_throttle(float target_speed) {
throttle *= reduction; throttle *= reduction;
if (in_reverse) { if (in_reverse) {
channel_throttle->set_servo_out(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
} else { } else {
channel_throttle->set_servo_out(constrain_int16(throttle, g.throttle_min, g.throttle_max)); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(throttle, g.throttle_min, g.throttle_max));
} }
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
@ -161,7 +164,7 @@ void Rover::calc_throttle(float target_speed) {
// is 2*braking_speederr // is 2*braking_speederr
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
channel_throttle->set_servo_out(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
// temporarily set us in reverse to allow the PWM setting to // temporarily set us in reverse to allow the PWM setting to
// go negative // go negative
@ -169,7 +172,7 @@ void Rover::calc_throttle(float target_speed) {
} }
if (use_pivot_steering()) { if (use_pivot_steering()) {
channel_throttle->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
} }
} }
@ -219,7 +222,7 @@ void Rover::calc_lateral_acceleration() {
void Rover::calc_nav_steer() { void Rover::calc_nav_steer() {
// check to see if the rover is loitering // check to see if the rover is loitering
if (in_stationary_loiter()) { if (in_stationary_loiter()) {
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
return; return;
} }
@ -231,68 +234,61 @@ void Rover::calc_nav_steer() {
// constrain to max G force // constrain to max G force
lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS); lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
channel_steer->set_servo_out(steerController.get_steering_out_lat_accel(lateral_acceleration)); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steerController.get_steering_out_lat_accel(lateral_acceleration));
} }
/***************************************** /*****************************************
Set the flight control servos based on the current calculated values Set the flight control servos based on the current calculated values
*****************************************/ *****************************************/
void Rover::set_servos(void) { void Rover::set_servos(void) {
static int16_t last_throttle; static uint16_t last_throttle;
// support a separate steering channel
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if (control_mode == MANUAL || control_mode == LEARNING) { if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values // do a direct pass through of radio values
channel_steer->set_radio_out(channel_steer->read()); SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->read());
channel_throttle->set_radio_out(channel_throttle->read()); SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual // suppress throttle if in failsafe and manual
channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim());
// suppress steer if in failsafe and manual and skid steer mode // suppress steer if in failsafe and manual and skid steer mode
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_radio_out(channel_steer->get_radio_trim()); SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim());
} }
} }
} else { } else {
channel_steer->calc_pwm();
if (in_reverse) { if (in_reverse) {
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-g.throttle_max, -g.throttle_max,
-g.throttle_min)); -g.throttle_min));
} else { } else {
channel_throttle->set_servo_out(constrain_int16(channel_throttle->get_servo_out(), SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g.throttle_min.get(), g.throttle_min.get(),
g.throttle_max.get())); g.throttle_max.get()));
} }
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe // suppress throttle if in failsafe
channel_throttle->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
// suppress steer if in failsafe and skid steer mode // suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
} }
} }
if (!hal.util->get_soft_armed()) { if (!hal.util->get_soft_armed()) {
channel_throttle->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
// suppress steer if in failsafe and skid steer mode // suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
} }
} }
// convert 0 to 100% into PWM
channel_throttle->calc_pwm();
// limit throttle movement speed // limit throttle movement speed
throttle_slew_limit(last_throttle); throttle_slew_limit(last_throttle);
} }
// record last throttle before we apply skid steering // record last throttle before we apply skid steering
last_throttle = channel_throttle->get_radio_out(); SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, last_throttle);
if (g.skid_steer_out) { if (g.skid_steer_out) {
// convert the two radio_out values to skid steering values // convert the two radio_out values to skid steering values
@ -303,14 +299,12 @@ void Rover::set_servos(void) {
motor1 = throttle + 0.5*steering motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering motor2 = throttle - 0.5*steering
*/ */
float steering_scaled = channel_steer->norm_output(); float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
float throttle_scaled = channel_throttle->norm_output(); float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f*steering_scaled; float motor1 = throttle_scaled + 0.5f*steering_scaled;
float motor2 = throttle_scaled - 0.5f*steering_scaled; float motor2 = throttle_scaled - 0.5f*steering_scaled;
channel_steer->set_servo_out(4500*motor1); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,4500*motor1);
channel_throttle->set_servo_out(100*motor2); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,100*motor2);
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
} }
if (!arming.is_armed()) { if (!arming.is_armed()) {
@ -323,28 +317,28 @@ void Rover::set_servos(void) {
break; break;
case AP_Arming::YES_ZERO_PWM: case AP_Arming::YES_ZERO_PWM:
channel_throttle->set_radio_out(0); SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,0);
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_radio_out(0); SRV_Channels::set_output_pwm(SRV_Channel::k_steering,0);
} }
break; break;
case AP_Arming::YES_MIN_PWM: case AP_Arming::YES_MIN_PWM:
default: default:
channel_throttle->set_radio_out(channel_throttle->get_radio_trim()); SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim());
if (g.skid_steer_out) { if (g.skid_steer_out) {
channel_steer->set_radio_out(channel_steer->get_radio_trim()); SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim());
} }
break; break;
} }
} }
SRV_Channels::calc_pwm();
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output // send values to the PWM timers for output
// ---------------------------------------- // ----------------------------------------
channel_steer->output(); SRV_Channels::output_ch_all();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
#endif #endif
} }

6
APMrover2/commands_logic.cpp
View File

@ -370,14 +370,14 @@ void Rover::nav_set_yaw_speed()
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_yaw_speed.msg_time_ms) > 3000) { if ((millis() - guided_yaw_speed.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
channel_throttle->set_servo_out(g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
channel_steer->set_servo_out(0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0; lateral_acceleration = 0;
return; return;
} }
int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle); int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
channel_steer->set_servo_out(steering); SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steering);
// speed param in the message gives speed as a proportion of cruise speed. // speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed // 0.5 would set speed to the cruise speed

2
APMrover2/control_modes.cpp
View File

@ -140,7 +140,7 @@ bool Rover::motor_active()
{ {
// Check if armed and throttle is not neutral // Check if armed and throttle is not neutral
if (hal.util->get_soft_armed()) { if (hal.util->get_soft_armed()) {
if (channel_throttle->get_servo_out() != channel_throttle->get_radio_trim()) { if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != channel_throttle->get_radio_trim()) {
return true; return true;
} }
} }

2
APMrover2/crash_check.cpp
View File

@ -21,7 +21,7 @@ void Rover::crash_check()
// TODO : Check if min vel can be calculated // TODO : Check if min vel can be calculated
// min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise;
if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(channel_throttle->norm_output())) < CRASH_CHECK_THROTTLE_MIN)) { if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))) < CRASH_CHECK_THROTTLE_MIN)) {
crash_counter = 0; crash_counter = 0;
return; return;
} }

2
APMrover2/failsafe.cpp
View File

@ -46,7 +46,7 @@ void Rover::failsafe_check()
for (uint8_t ch=start_ch; ch < 4; ch++) { for (uint8_t ch=start_ch; ch < 4; ch++) {
hal.rcout->write(ch, hal.rcin->read(ch)); hal.rcout->write(ch, hal.rcin->read(ch));
} }
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
} }
} }

36
APMrover2/radio.cpp
View File

@ -5,14 +5,20 @@
*/ */
void Rover::set_control_channels(void) void Rover::set_control_channels(void)
{ {
channel_steer = RC_Channel::rc_channel(rcmap.roll()-1); channel_steer = RC_Channels::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
channel_learn = RC_Channel::rc_channel(g.learn_channel-1); channel_learn = RC_Channels::rc_channel(g.learn_channel-1);
SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering);
SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle);
// set rc channel ranges // set rc channel ranges
channel_steer->set_angle(SERVO_MAX); channel_steer->set_angle(SERVO_MAX);
channel_throttle->set_angle(100); channel_throttle->set_angle(100);
SRV_Channels::set_angle(SRV_Channel::k_steering, SERVO_MAX);
SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
// For a rover safety is TRIM throttle // For a rover safety is TRIM throttle
if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) { if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim()); hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim());
@ -37,20 +43,10 @@ void Rover::init_rc_in()
void Rover::init_rc_out() void Rover::init_rc_out()
{ {
RC_Channel::rc_channel(CH_1)->enable_out(); SRV_Channels::output_trim_all();
RC_Channel::rc_channel(CH_3)->enable_out();
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
if (g.skid_steer_out) {
channel_steer->enable_out();
}
}
RC_Channel::output_trim_all();
// setup PWM values to send if the FMU firmware dies // setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all(); SRV_Channels::setup_failsafe_trim_all();
// output throttle trim when safety off if arming // output throttle trim when safety off if arming
// is setup for min on disarm. MIN is from plane where MIN is effectively no throttle. // is setup for min on disarm. MIN is from plane where MIN is effectively no throttle.
@ -134,17 +130,17 @@ void Rover::read_radio()
failsafe.last_valid_rc_ms = AP_HAL::millis(); failsafe.last_valid_rc_ms = AP_HAL::millis();
RC_Channel::set_pwm_all(); RC_Channels::set_pwm_all();
control_failsafe(channel_throttle->get_radio_in()); control_failsafe(channel_throttle->get_radio_in());
channel_throttle->set_servo_out(channel_throttle->get_control_in()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
// Check if the throttle value is above 50% and we need to nudge // Check if the throttle value is above 50% and we need to nudge
// Make sure its above 50% in the direction we are travelling // Make sure its above 50% in the direction we are travelling
if ((abs(channel_throttle->get_servo_out()) > 50) && if ((abs(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) > 50) &&
(((channel_throttle->get_servo_out() < 0) && in_reverse) || (((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) ||
((channel_throttle->get_servo_out() > 0) && !in_reverse))) { ((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * throttle_nudge = (g.throttle_max - g.throttle_cruise) *
((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f); ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
} else { } else {

9
APMrover2/system.cpp
View File

@ -510,9 +510,6 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
return false; return false;
} }
// only log if arming was successful
channel_throttle->enable_out();
change_arm_state(); change_arm_state();
return true; return true;
} }
@ -525,12 +522,6 @@ bool Rover::disarm_motors(void)
if (!arming.disarm()) { if (!arming.disarm()) {
return false; return false;
} }
if (arming.arming_required() == AP_Arming::YES_ZERO_PWM) {
channel_throttle->disable_out();
if (g.skid_steer_out) {
channel_steer->disable_out();
}
}
if (control_mode != AUTO) { if (control_mode != AUTO) {
// reset the mission on disarm if we are not in auto // reset the mission on disarm if we are not in auto
mission.reset(); mission.reset();

66
APMrover2/test.cpp
View File

@ -7,8 +7,6 @@
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details // See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] = { static const struct Menu::command test_menu_commands[] = {
{"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", MENU_FUNC(test_radio)},
{"passthru", MENU_FUNC(test_passthru)}, {"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)}, {"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)}, {"relay", MENU_FUNC(test_relay)},
@ -42,35 +40,6 @@ void Rover::print_hit_enter()
cliSerial->printf("Hit Enter to exit.\n\n"); cliSerial->printf("Hit Enter to exit.\n\n");
} }
int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while (1) {
delay(20);
// Filters radio input - adjust filters in the radio.cpp file
// ----------------------------------------------------------
read_radio();
cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_radio_in(),
g.rc_2.get_radio_in(),
channel_throttle->get_radio_in(),
g.rc_4.get_radio_in(),
g.rc_5.get_radio_in(),
g.rc_6.get_radio_in(),
g.rc_7.get_radio_in(),
g.rc_8.get_radio_in());
if (cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv) int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
@ -96,41 +65,6 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
// read the radio to set trims
// ---------------------------
trim_radio();
while (1) {
delay(20);
read_radio();
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n",
channel_steer->get_control_in(),
g.rc_2.get_control_in(),
channel_throttle->get_control_in(),
g.rc_4.get_control_in(),
g.rc_5.get_control_in(),
g.rc_6.get_control_in(),
g.rc_7.get_control_in(),
g.rc_8.get_control_in());
if (cliSerial->available() > 0) {
return (0);
}
}
}
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{ {