ardupilot/APMrover2/radio.cpp

159 lines
4.6 KiB
C++
Raw Normal View History

2015-05-13 00:16:45 -03:00
#include "Rover.h"
/*
allow for runtime change of control channel ordering
*/
void Rover::set_control_channels(void)
{
// check change on RCMAP
2017-01-06 06:31:10 -04:00
channel_steer = RC_Channels::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
channel_aux = RC_Channels::rc_channel(g.aux_channel-1);
channel_lateral = RC_Channels::rc_channel(rcmap.yaw()-1);
// set rc channel ranges
channel_steer->set_angle(SERVO_MAX);
channel_throttle->set_angle(100);
channel_lateral->set_angle(100);
// Allow to reconfigure ouput when not armed
if (!arming.is_armed()) {
g2.motors.setup_servo_output();
// For a rover safety is TRIM throttle
g2.motors.setup_safety_output();
}
2014-11-20 03:32:08 -04:00
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed. Default to 1000 to 2000 for systems without
// a k_throttle output
hal.rcout->set_esc_scaling(1000, 2000);
g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
}
void Rover::init_rc_in()
{
// set rc dead zones
channel_steer->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
channel_lateral->set_default_dead_zone(30);
}
void Rover::init_rc_out()
{
2017-07-10 06:43:55 -03:00
// set auxiliary ranges
update_aux();
}
/*
check for driver input on rudder/steering stick for arming/disarming
*/
void Rover::rudder_arm_disarm_check()
{
// In Rover we need to check that its set to the throttle trim and within the DZ
// if throttle is not within trim dz, then pilot cannot rudder arm/disarm
if (!channel_throttle->in_trim_dz()) {
rudder_arm_timer = 0;
return;
}
// check if arming/disarming allowed from this mode
if (!control_mode->allows_arming_from_transmitter()) {
rudder_arm_timer = 0;
return;
}
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
if (channel_steer->get_control_in() > 4000) {
2017-01-31 06:12:26 -04:00
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
2017-11-30 21:54:45 -04:00
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to arm!
arm_motors(AP_Arming::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else if (!g2.motors.active()) {
// when armed and motor not active (not moving), full left rudder starts disarming counter
if (channel_steer->get_control_in() < -4000) {
2017-01-31 06:12:26 -04:00
const uint32_t now = millis();
if (rudder_arm_timer == 0 ||
2017-11-30 21:54:45 -04:00
now - rudder_arm_timer < ARM_DELAY_MS) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
// time to disarm!
disarm_motors();
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
}
void Rover::read_radio()
{
if (!RC_Channels::read_input()) {
2017-06-19 12:47:13 -03:00
// check if we lost RC link
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
control_failsafe(channel_throttle->get_radio_in());
return;
}
failsafe.last_valid_rc_ms = AP_HAL::millis();
2017-06-19 12:47:13 -03:00
// check that RC value are valid
control_failsafe(channel_throttle->get_radio_in());
2017-06-19 12:47:13 -03:00
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}
void Rover::control_failsafe(uint16_t pwm)
{
if (!g.fs_throttle_enabled) {
// no throttle failsafe
return;
}
// Check for failsafe condition based on loss of GCS control
if (rc_override_active) {
failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
} else if (g.fs_throttle_enabled) {
2017-03-30 11:07:24 -03:00
bool failed = pwm < static_cast<uint16_t>(g.fs_throttle_value);
if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 2000) {
failed = true;
}
failsafe_trigger(FAILSAFE_EVENT_THROTTLE, failed);
}
}
void Rover::trim_control_surfaces()
{
read_radio();
// Store control surface trim values
// ---------------------------------
if ((channel_steer->get_radio_in() > 1400) && (channel_steer->get_radio_in() < 1600)) {
channel_steer->set_radio_trim(channel_steer->get_radio_in());
// save to eeprom
channel_steer->save_eeprom();
}
}
void Rover::trim_radio()
{
for (uint8_t y = 0; y < 30; y++) {
read_radio();
}
trim_control_surfaces();
}