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

Rover: integrate motors library 

move throttle_slew_limit and THR_SLEWRATE parameter
move have_skid_steering to library
move mix_skid_steering to library's output_skid_steering method
move radio.cpp's output channel initialisation to motor's init method
use motors.get_throttle and get_steering instead of getting from Servo objects
use motors.set_throttle and set_steering instead of setting to Servo object
AP_Arming::arming_required is replaced with SAFE_DISARM parameter
This commit is contained in:
Pierre Kancir 2017-07-06 12:06:20 +09:00 committed by Randy Mackay
parent 29c59644b7
commit 267a1532b9
12 changed files with 64 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
APMrover2/APMrover2.cpp
View File

@ -438,18 +438,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 (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(rover.guided_control.target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0.0f));
Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f));
}
break;
@ -501,18 +501,18 @@ void Rover::update_current_mode(void)
we set the exact value in set_servos(), but it helps for
logging
*/
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->get_control_in());
g2.motors.set_throttle(channel_throttle->get_control_in());
g2.motors.set_steering(channel_steer->get_control_in());
// mark us as in_reverse when using a negative throttle to
// stop AHRS getting off
set_reverse(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0);
set_reverse(is_negative(g2.motors.get_throttle()));
break;
case HOLD:
// hold position - stop motors and center steering
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_throttle(0);
g2.motors.set_steering(0);
if (!in_auto_reverse) {
set_reverse(false);
}
@ -543,7 +543,7 @@ void Rover::update_navigation()
case RTL:
// no loitering around the wp with the rover, goes direct to the wp position
if (verify_RTL()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
g2.motors.set_throttle(g.throttle_min.get());
set_mode(HOLD);
} else {
calc_lateral_acceleration();
@ -561,8 +561,8 @@ void Rover::update_navigation()
// no loitering around the wp with the rover, goes direct to the wp position
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();

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,
100 * SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g2.motors.get_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,
SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g2.motors.get_throttle(),
current_loc.alt / 100.0f,
0);
}

4
APMrover2/Log.cpp
View File

@ -248,10 +248,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)SRV_Channels::get_output_scaled(SRV_Channel::k_steering),
steer_out : (int16_t)g2.motors.get_steering(),
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
throttle_out : (int16_t)g2.motors.get_throttle(),
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));

13
APMrover2/Parameters.cpp
View File

@ -202,15 +202,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(throttle_cruise, "CRUISE_THROTTLE", 50),
// @Param: THR_SLEWRATE
// @DisplayName: Throttle slew rate
// @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout.
// @Units: %/s
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 100),
// @Param: SKID_STEER_IN
// @DisplayName: Skid steering input
// @Description: Set this to 1 for skid steering input rovers (tank track style in RC controller). When enabled, servo1 is used for the left track control, servo3 is used for right track control
@ -551,6 +542,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp
AP_SUBGROUPINFO(visual_odom, "VISO", 7, ParametersG2, AP_VisualOdom),
// @Group: MOT_
// @Path: MotorsUGV.cpp
AP_SUBGROUPINFO(motors, "MOT_", 8, ParametersG2, AP_MotorsUGV),
AP_GROUPEND
};

6
APMrover2/Parameters.h
View File

@ -127,7 +127,7 @@ public:
k_param_throttle_min = 170,
k_param_throttle_max,
k_param_throttle_cruise,
k_param_throttle_slewrate,
k_param_throttle_slewrate_old,
k_param_throttle_reduction,
k_param_skid_steer_in,
k_param_skid_steer_out_old,
@ -248,7 +248,6 @@ public:
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_cruise;
AP_Int8 throttle_slewrate;
AP_Int8 skid_steer_in;
// failsafe control
@ -322,6 +321,9 @@ public:
// Visual Odometry camera
AP_VisualOdom visual_odom;
// Motor library
AP_MotorsUGV motors;
};
extern const AP_Param::Info var_info[];

4
APMrover2/Rover.h
View File

@ -63,6 +63,7 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#include "AP_MotorsUGV.h"
#include "AP_Arming.h"
#include "compat.h"
@ -478,14 +479,11 @@ private:
void Log_Arm_Disarm();
void load_parameters(void);
void throttle_slew_limit(void);
bool auto_check_trigger(void);
bool use_pivot_steering(void);
void calc_throttle(float target_speed);
void calc_lateral_acceleration();
void calc_nav_steer();
bool have_skid_steering();
void mix_skid_steering();
void set_servos(void);
void set_auto_WP(const struct Location& loc);
void set_guided_WP(const struct Location& loc);

165
APMrover2/Steering.cpp
View File

@ -1,18 +1,5 @@
#include "Rover.h"
/*****************************************
Throttle slew limit
*****************************************/
void Rover::throttle_slew_limit(void) {
if (g.throttle_slewrate > 0) {
SRV_Channels::limit_slew_rate(SRV_Channel::k_throttle, g.throttle_slewrate, G_Dt);
if (have_skid_steering()) {
// when skid steering also limit 2nd channel
SRV_Channels::limit_slew_rate(SRV_Channel::k_steering, g.throttle_slewrate, G_Dt);
}
}
}
/*
check for triggering of start of auto mode
*/
@ -65,7 +52,7 @@ bool Rover::auto_check_trigger(void) {
bool Rover::use_pivot_steering(void)
{
// check cases where we clearly cannot use pivot steering
if (control_mode < AUTO || !have_skid_steering() || g.pivot_turn_angle <= 0) {
if (control_mode < AUTO || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
pivot_steering_active = false;
return false;
}
@ -114,10 +101,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()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
g2.motors.set_throttle(g.throttle_min.get());
// Stop rotation in case of loitering and skid steering
if (have_skid_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
if (g2.motors.have_skid_steering()) {
g2.motors.set_steering(0);
}
return;
}
@ -160,9 +147,9 @@ void Rover::calc_throttle(float target_speed) {
throttle *= reduction;
if (in_reverse) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max));
g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
}
if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
@ -175,7 +162,7 @@ void Rover::calc_throttle(float target_speed) {
// is 2*braking_speederr
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
// temporarily set us in reverse to allow the PWM setting to
// go negative
@ -185,7 +172,7 @@ void Rover::calc_throttle(float target_speed) {
if (guided_mode != Guided_Velocity) {
if (use_pivot_steering()) {
// In Guided Velocity, only the steering input is used to calculate the pivot turn.
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
g2.motors.set_throttle(0);
}
}
}
@ -236,7 +223,7 @@ void Rover::calc_lateral_acceleration() {
void Rover::calc_nav_steer() {
// check to see if the rover is loitering
if (in_stationary_loiter()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_steering(0);
return;
}
@ -248,142 +235,20 @@ 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);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steerController.get_steering_out_lat_accel(lateral_acceleration));
}
/*
run the skid steering mixer
*/
void Rover::mix_skid_steering(void)
{
float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f; // steering scaled -1 to +1
float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f; // throttle scaled -1 to +1
// apply constraints
steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f);
throttle_scaled = constrain_float(throttle_scaled, -1.0f, 1.0f);
// check for saturation and scale back throttle and steering proportionally
const float saturation_value = fabsf(steering_scaled) + fabsf(throttle_scaled);
if (saturation_value > 1.0f) {
steering_scaled = steering_scaled / saturation_value;
throttle_scaled = throttle_scaled / saturation_value;
}
// add in throttle
float motor_left = throttle_scaled;
float motor_right = throttle_scaled;
// deal with case of turning on the spot
if (is_zero(throttle_scaled)) {
// full possible range is not used to keep response equivalent to non-zero throttle case
motor_left += steering_scaled * 0.5f;
motor_right -= steering_scaled * 0.5f;
} else {
// add in steering
const float dir = is_positive(throttle_scaled) ? 1.0f : -1.0f;
if (is_negative(steering_scaled)) {
// moving left all steering to right wheel
motor_right -= dir * steering_scaled;
} else {
// turning right, all steering to left wheel
motor_left += dir * steering_scaled;
}
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motor_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor_right);
g2.motors.set_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) {
if (in_reverse) {
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 {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
g.throttle_min,
g.throttle_max));
}
// Check Throttle failsafe in non auto mode. Suppress all ouput
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (have_skid_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
// Check if soft arm. Suppress all ouput
if (!hal.util->get_soft_armed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
// suppress steer if in failsafe and skid steer mode
if (have_skid_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
}
}
// Apply slew rate limit on non Manual modes
if (control_mode != MANUAL && control_mode != LEARNING) {
// limit throttle movement speed
throttle_slew_limit();
}
// Apply skid steering mixing
if (have_skid_steering()) {
mix_skid_steering();
g2.motors.slew_limit_throttle(true);
} else {
g2.motors.slew_limit_throttle(false);
}
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:
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
if (have_skid_steering()) {
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
}
break;
case AP_Arming::YES_MIN_PWM:
default:
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
if (have_skid_steering()) {
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
break;
}
}
SRV_Channels::calc_pwm();
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
hal.rcout->cork();
SRV_Channels::output_ch_all();
hal.rcout->push();
#endif
}
/*
work out if skid steering is available
*/
bool Rover::have_skid_steering(void)
{
if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) &&
SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) {
return true;
}
return false;
// send output signals to motors
g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
}

20
APMrover2/commands_logic.cpp
View File

@ -377,14 +377,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_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0);
lateral_acceleration = 0.0f;
return;
}
const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
g2.motors.set_steering(steering);
// speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed
@ -400,8 +400,8 @@ void Rover::nav_set_speed()
// if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0);
lateral_acceleration = 0.0f;
prev_WP = current_loc;
next_WP = current_loc;
@ -415,7 +415,7 @@ void Rover::nav_set_speed()
location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f); // put the next wp at 4m forward at steer direction
nav_controller->update_waypoint(current_loc, next_WP);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
g2.motors.set_steering(steer_value);
calc_throttle(guided_control.target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
@ -459,7 +459,7 @@ void Rover::do_yaw(const AP_Mission::Mission_Command& cmd)
// Calculate the steering to apply base on error calculated
const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
g2.motors.set_steering(steering);
next_navigation_leg_cd = condition_value;
calc_throttle(g.speed_cruise);
@ -473,16 +473,16 @@ bool Rover::do_yaw_rotation()
// check if we are within 5 degrees of the target heading
if (error_to_target_yaw <= 500) {
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); // stop the current rotation
g2.motors.set_steering(0); // stop the current rotation
condition_value = condition_start; // reset the condition value to its previous value
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
g2.motors.set_throttle(0);
next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
do_auto_rotation = false;
return true;
} else {
// Calculate the steering to apply base on error calculated
const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
g2.motors.set_steering(steering);
calc_throttle(g.speed_cruise);
do_auto_rotation = true;
return false;

2
APMrover2/control_modes.cpp
View File

@ -140,7 +140,7 @@ bool Rover::motor_active()
{
// Check if armed and output throttle servo is not neutral
if (hal.util->get_soft_armed()) {
if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0) {
if (!is_zero(g2.motors.get_throttle())) {
return true;
}
}

2
APMrover2/crash_check.cpp
View File

@ -23,7 +23,7 @@ void Rover::crash_check()
if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity
(fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed
(fabsf(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) < CRASH_CHECK_THROTTLE_MIN)) {
(fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN)) {
crash_counter = 0;
return;
}

38
APMrover2/radio.cpp
View File

@ -5,6 +5,7 @@
*/
void Rover::set_control_channels(void)
{
// check change on RCMAP
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);
@ -13,19 +14,9 @@ void Rover::set_control_channels(void)
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);
// left/right throttle as -1000 to 1000 values
SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 1000);
SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 1000);
// For a rover safety is TRIM throttle
if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
if (have_skid_steering()) {
SRV_Channels::set_safety_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
if (!arming.is_armed()) {
g2.motors.setup_safety_output();
}
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed. Default to 1000 to 2000 for systems without
@ -50,17 +41,6 @@ void Rover::init_rc_out()
// setup PWM values to send if the FMU firmware dies
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.
// For Rover's no throttle means TRIM as rovers can go backwards i.e. MIN throttle is
// full speed backward.
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
if (have_skid_steering()) {
SRV_Channels::set_safety_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
}
}
}
/*
@ -100,7 +80,7 @@ void Rover::rudder_arm_disarm_check()
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!motor_active() & !have_skid_steering()) {
} else if (!motor_active() & !g2.motors.have_skid_steering()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
// This is disabled for skid steering otherwise when tring to turn a skid steering rover around
// the rover would disarm
@ -138,14 +118,14 @@ void Rover::read_radio()
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
// copy RC scaled inputs to outputs
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->get_control_in());
g2.motors.set_throttle(channel_throttle->get_control_in());
g2.motors.set_steering(channel_steer->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(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))) {
if ((fabs(g2.motors.get_throttle()) > 50) &&
(((g2.motors.get_throttle() < 0) && in_reverse) ||
((g2.motors.get_throttle() > 0) && !in_reverse))) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) *
((fabsf(channel_throttle->norm_input()) - 0.5f) / 0.5f);
} else {

3
APMrover2/system.cpp
View File

@ -180,6 +180,9 @@ void Rover::init_ardupilot()
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
// init motors including setting rc out channels ranges
g2.motors.init();
relay.init();
#if MOUNT == ENABLED