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)
{
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
}
/*
@ -452,17 +452,18 @@ void Rover::update_current_mode(void)
case Guided_WP:
if (rtl_complete || verify_RTL()) {
// 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);
}
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
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;
@ -510,18 +511,18 @@ void Rover::update_current_mode(void)
we set the exact value in set_servos(), but it helps for
logging
*/
channel_throttle->set_servo_out(channel_throttle->get_control_in());
channel_steer->set_servo_out(channel_steer->pwm_to_angle());
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
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
// 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;
case HOLD:
// hold position - stop motors and center steering
channel_throttle->set_servo_out(0);
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
break;
case INITIALISING:
@ -548,7 +549,7 @@ void Rover::update_navigation()
calc_lateral_acceleration();
calc_nav_steer();
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);
}
break;
@ -565,8 +566,8 @@ void Rover::update_navigation()
calc_nav_steer();
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0;
}
break;

4
APMrover2/GCS_Mavlink.cpp
View File

@ -162,7 +162,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
0, // port 0
10000 * channel_steer->norm_output(),
0,
10000 * channel_throttle->norm_output(),
10000 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle),
0,
0,
0,
@ -179,7 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
gps.ground_speed(),
ahrs.groundspeed(),
(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,
0);
}

8
APMrover2/Log.cpp
View File

@ -238,10 +238,10 @@ void Rover::Log_Write_Control_Tuning()
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
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,
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
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -268,7 +268,7 @@ void Rover::Log_Write_Nav_Tuning()
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_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()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -326,7 +326,7 @@ void Rover::Log_Write_Sonar()
turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time,
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));
}

69
APMrover2/Parameters.cpp
View File

@ -174,62 +174,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
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
// @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.
@ -585,10 +529,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
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
};
ParametersG2::ParametersG2(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
/*
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_BoardConfig,
k_param_pivot_turn_angle,
k_param_rc_13,
k_param_rc_14,
k_param_rc_13_old,
k_param_rc_14_old,
// IO pins
k_param_rssi_pin = 20, // unused, replaced by rssi_ library parameters
@ -113,14 +113,14 @@ public:
//
// 160: Radio settings
//
k_param_rc_1 = 160,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_1_old = 160,
k_param_rc_2_old,
k_param_rc_3_old,
k_param_rc_4_old,
k_param_rc_5_old,
k_param_rc_6_old,
k_param_rc_7_old,
k_param_rc_8_old,
// throttle control
k_param_throttle_min = 170,
@ -182,10 +182,10 @@ public:
k_param_pidSpeedThrottle,
// high RC channels
k_param_rc_9 = 235,
k_param_rc_10,
k_param_rc_11,
k_param_rc_12,
k_param_rc_9_old = 235,
k_param_rc_10_old,
k_param_rc_11_old,
k_param_rc_12_old,
// other objects
k_param_sitl = 240,
@ -242,22 +242,6 @@ public:
AP_Int16 pivot_turn_angle;
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
//
AP_Int8 throttle_min;
@ -302,22 +286,6 @@ public:
PID pidSpeedThrottle;
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
//-----------------------------------------------------------------------------------
pidSpeedThrottle (0.7, 0.2, 0.2, 4000)
@ -329,7 +297,7 @@ public:
*/
class ParametersG2 {
public:
ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); }
ParametersG2(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -339,6 +307,12 @@ public:
// whether to enforce acceptance of packets only from sysid_my_gcs
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[];

1
APMrover2/Rover.h
View File

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

76
APMrover2/Steering.cpp
View File

@ -12,7 +12,10 @@ void Rover::throttle_slew_limit(int16_t last_throttle) {
if (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
// then set the throttle to minimum
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
if (g.skid_steer_out) {
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
}
return;
}
@ -146,9 +149,9 @@ void Rover::calc_throttle(float target_speed) {
throttle *= reduction;
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 {
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) {
@ -161,7 +164,7 @@ void Rover::calc_throttle(float target_speed) {
// is 2*braking_speederr
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;
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
// go negative
@ -169,7 +172,7 @@ void Rover::calc_throttle(float target_speed) {
}
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() {
// check to see if the rover is loitering
if (in_stationary_loiter()) {
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
return;
}
@ -231,68 +234,61 @@ void Rover::calc_nav_steer() {
// constrain to max G force
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
*****************************************/
void Rover::set_servos(void) {
static int16_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));
static uint16_t last_throttle;
if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values
channel_steer->set_radio_out(channel_steer->read());
channel_throttle->set_radio_out(channel_throttle->read());
SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->read());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// 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
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 {
channel_steer->calc_pwm();
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_min));
} 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_max.get()));
}
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// 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
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()) {
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
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
throttle_slew_limit(last_throttle);
}
// 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) {
// convert the two radio_out values to skid steering values
@ -303,14 +299,12 @@ void Rover::set_servos(void) {
motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering
*/
float steering_scaled = channel_steer->norm_output();
float throttle_scaled = channel_throttle->norm_output();
float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f*steering_scaled;
float motor2 = throttle_scaled - 0.5f*steering_scaled;
channel_steer->set_servo_out(4500*motor1);
channel_throttle->set_servo_out(100*motor2);
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,4500*motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,100*motor2);
}
if (!arming.is_armed()) {
@ -323,28 +317,28 @@ void Rover::set_servos(void) {
break;
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) {
channel_steer->set_radio_out(0);
SRV_Channels::set_output_pwm(SRV_Channel::k_steering,0);
}
break;
case AP_Arming::YES_MIN_PWM:
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) {
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;
}
}
SRV_Channels::calc_pwm();
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
channel_steer->output();
channel_throttle->output();
RC_Channel_aux::output_ch_all();
SRV_Channels::output_ch_all();
#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 ((millis() - guided_yaw_speed.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
lateral_acceleration = 0;
return;
}
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.
// 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
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;
}
}

2
APMrover2/crash_check.cpp
View File

@ -21,7 +21,7 @@ void Rover::crash_check()
// TODO : Check if min vel can be calculated
// 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;
return;
}

2
APMrover2/failsafe.cpp
View File

@ -46,7 +46,7 @@ void Rover::failsafe_check()
for (uint8_t ch=start_ch; ch < 4; 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)
{
channel_steer = RC_Channel::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
channel_learn = RC_Channel::rc_channel(g.learn_channel-1);
channel_steer = RC_Channels::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-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
channel_steer->set_angle(SERVO_MAX);
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
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());
@ -37,20 +43,10 @@ void Rover::init_rc_in()
void Rover::init_rc_out()
{
RC_Channel::rc_channel(CH_1)->enable_out();
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();
SRV_Channels::output_trim_all();
// 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
// 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();
RC_Channel::set_pwm_all();
RC_Channels::set_pwm_all();
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
// Make sure its above 50% in the direction we are travelling
if ((abs(channel_throttle->get_servo_out()) > 50) &&
(((channel_throttle->get_servo_out() < 0) && in_reverse) ||
((channel_throttle->get_servo_out() > 0) && !in_reverse))) {
if ((abs(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) > 50) &&
(((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) ||
((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) *
((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
} else {

9
APMrover2/system.cpp
View File

@ -510,9 +510,6 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
return false;
}
// only log if arming was successful
channel_throttle->enable_out();
change_arm_state();
return true;
}
@ -525,12 +522,6 @@ bool Rover::disarm_motors(void)
if (!arming.disarm()) {
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) {
// reset the mission on disarm if we are not in auto
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.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] = {
{"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", MENU_FUNC(test_radio)},
{"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)},
@ -42,35 +40,6 @@ void Rover::print_hit_enter()
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)
{
print_hit_enter();
@ -96,41 +65,6 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
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)
{