|
|
|
@ -9,8 +9,8 @@ void Rover::set_control_channels(void)
@@ -9,8 +9,8 @@ void Rover::set_control_channels(void)
|
|
|
|
|
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); |
|
|
|
|
channel_learn = RC_Channel::rc_channel(g.learn_channel-1); |
|
|
|
|
|
|
|
|
|
// set rc channel ranges
|
|
|
|
|
channel_steer->set_angle(SERVO_MAX); |
|
|
|
|
// set rc channel ranges
|
|
|
|
|
channel_steer->set_angle(SERVO_MAX); |
|
|
|
|
channel_throttle->set_angle(100); |
|
|
|
|
|
|
|
|
|
// For a rover safety is TRIM throttle
|
|
|
|
@ -20,19 +20,18 @@ void Rover::set_control_channels(void)
@@ -20,19 +20,18 @@ void Rover::set_control_channels(void)
|
|
|
|
|
hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1), channel_steer->get_radio_trim()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
|
|
|
|
|
// take a proportion of speed.
|
|
|
|
|
// take a proportion of speed.
|
|
|
|
|
hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::init_rc_in() |
|
|
|
|
{ |
|
|
|
|
// set rc dead zones
|
|
|
|
|
channel_steer->set_default_dead_zone(30); |
|
|
|
|
channel_throttle->set_default_dead_zone(30); |
|
|
|
|
// set rc dead zones
|
|
|
|
|
channel_steer->set_default_dead_zone(30); |
|
|
|
|
channel_throttle->set_default_dead_zone(30); |
|
|
|
|
|
|
|
|
|
//set auxiliary ranges
|
|
|
|
|
// set auxiliary ranges
|
|
|
|
|
update_aux(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -48,10 +47,10 @@ void Rover::init_rc_out()
@@ -48,10 +47,10 @@ void Rover::init_rc_out()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
RC_Channel::output_trim_all();
|
|
|
|
|
RC_Channel::output_trim_all(); |
|
|
|
|
|
|
|
|
|
// setup PWM values to send if the FMU firmware dies
|
|
|
|
|
RC_Channel::setup_failsafe_trim_all();
|
|
|
|
|
RC_Channel::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.
|
|
|
|
@ -83,48 +82,47 @@ void Rover::rudder_arm_disarm_check()
@@ -83,48 +82,47 @@ void Rover::rudder_arm_disarm_check()
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!arming.is_armed()) { |
|
|
|
|
// when not armed, full right rudder starts arming counter
|
|
|
|
|
if (channel_steer->get_control_in() > 4000) { |
|
|
|
|
uint32_t now = millis(); |
|
|
|
|
|
|
|
|
|
if (rudder_arm_timer == 0 || |
|
|
|
|
now - rudder_arm_timer < 3000) { |
|
|
|
|
if (!arming.is_armed()) { |
|
|
|
|
// when not armed, full right rudder starts arming counter
|
|
|
|
|
if (channel_steer->get_control_in() > 4000) { |
|
|
|
|
uint32_t now = millis(); |
|
|
|
|
|
|
|
|
|
if (rudder_arm_timer == 0) { |
|
|
|
|
if (rudder_arm_timer == 0 || |
|
|
|
|
now - rudder_arm_timer < 3000) { |
|
|
|
|
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 (!motor_active() & !g.skid_steer_out) { |
|
|
|
|
// when armed and motor not active (not moving), full left rudder starts disarming counter
|
|
|
|
|
} 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 (!motor_active() & !g.skid_steer_out) { |
|
|
|
|
// 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
|
|
|
|
|
if (channel_steer->get_control_in() < -4000) { |
|
|
|
|
uint32_t now = millis(); |
|
|
|
|
if (channel_steer->get_control_in() < -4000) { |
|
|
|
|
uint32_t now = millis(); |
|
|
|
|
|
|
|
|
|
if (rudder_arm_timer == 0 || |
|
|
|
|
now - rudder_arm_timer < 3000) { |
|
|
|
|
if (rudder_arm_timer == 0) { |
|
|
|
|
if (rudder_arm_timer == 0 || |
|
|
|
|
now - rudder_arm_timer < 3000) { |
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
// time to disarm!
|
|
|
|
|
disarm_motors(); |
|
|
|
|
rudder_arm_timer = 0; |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
// not at full left rudder
|
|
|
|
|
rudder_arm_timer = 0; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::read_radio() |
|
|
|
@ -138,20 +136,20 @@ void Rover::read_radio()
@@ -138,20 +136,20 @@ void Rover::read_radio()
|
|
|
|
|
|
|
|
|
|
RC_Channel::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()); |
|
|
|
|
channel_throttle->set_servo_out(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) && |
|
|
|
|
if ((abs(channel_throttle->get_servo_out()) > 50) && |
|
|
|
|
(((channel_throttle->get_servo_out() < 0) && in_reverse) || |
|
|
|
|
((channel_throttle->get_servo_out() > 0) && !in_reverse))) { |
|
|
|
|
throttle_nudge = (g.throttle_max - g.throttle_cruise) * |
|
|
|
|
((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f); |
|
|
|
|
} else { |
|
|
|
|
throttle_nudge = 0; |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
throttle_nudge = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (g.skid_steer_in) { |
|
|
|
|
// convert the two radio_in values from skid steering values
|
|
|
|
@ -184,26 +182,25 @@ void Rover::read_radio()
@@ -184,26 +182,25 @@ void Rover::read_radio()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
rudder_arm_disarm_check(); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::control_failsafe(uint16_t pwm) |
|
|
|
|
{ |
|
|
|
|
if (!g.fs_throttle_enabled) { |
|
|
|
|
if (!g.fs_throttle_enabled) { |
|
|
|
|
// no throttle failsafe
|
|
|
|
|
return; |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Check for failsafe condition based on loss of GCS control
|
|
|
|
|
if (rc_override_active) { |
|
|
|
|
// 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) { |
|
|
|
|
} else if (g.fs_throttle_enabled) { |
|
|
|
|
bool failed = pwm < (uint16_t)g.fs_throttle_value; |
|
|
|
|
if (AP_HAL::millis() - failsafe.last_valid_rc_ms > 2000) { |
|
|
|
|
failed = true; |
|
|
|
|
} |
|
|
|
|
failsafe_trigger(FAILSAFE_EVENT_THROTTLE, failed); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
@ -227,11 +224,11 @@ bool Rover::throttle_failsafe_active(void)
@@ -227,11 +224,11 @@ bool Rover::throttle_failsafe_active(void)
|
|
|
|
|
|
|
|
|
|
void Rover::trim_control_surfaces() |
|
|
|
|
{ |
|
|
|
|
read_radio(); |
|
|
|
|
// Store control surface trim values
|
|
|
|
|
// ---------------------------------
|
|
|
|
|
read_radio(); |
|
|
|
|
// Store control surface trim values
|
|
|
|
|
// ---------------------------------
|
|
|
|
|
if (channel_steer->get_radio_in() > 1400) { |
|
|
|
|
channel_steer->set_radio_trim(channel_steer->get_radio_in()); |
|
|
|
|
channel_steer->set_radio_trim(channel_steer->get_radio_in()); |
|
|
|
|
// save to eeprom
|
|
|
|
|
channel_steer->save_eeprom(); |
|
|
|
|
} |
|
|
|
@ -239,8 +236,8 @@ void Rover::trim_control_surfaces()
@@ -239,8 +236,8 @@ void Rover::trim_control_surfaces()
|
|
|
|
|
|
|
|
|
|
void Rover::trim_radio() |
|
|
|
|
{ |
|
|
|
|
for (int y = 0; y < 30; y++) { |
|
|
|
|
read_radio(); |
|
|
|
|
} |
|
|
|
|
for (int y = 0; y < 30; y++) { |
|
|
|
|
read_radio(); |
|
|
|
|
} |
|
|
|
|
trim_control_surfaces(); |
|
|
|
|
} |
|
|
|
|