aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: radio.cpp correct whitespace, remove tabs

Browse Source
This commit is contained in:
Pierre Kancir 2016-12-20 14:33:36 +01:00 committed by Randy Mackay
parent fb8446ffd5
commit 25fbfeb5cf
1 changed files with 61 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
APMrover2/radio.cpp
View File

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