|
|
|
@ -28,7 +28,7 @@ void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
@@ -28,7 +28,7 @@ void RC_Channel_Rover::mode_switch_changed(modeswitch_pos_t new_pos)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// init_aux_switch_function - initialize aux functions
|
|
|
|
|
void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
// init channel options
|
|
|
|
|
switch (ch_option) { |
|
|
|
@ -74,16 +74,16 @@ RC_Channel * RC_Channels_Rover::get_arming_channel(void) const
@@ -74,16 +74,16 @@ RC_Channel * RC_Channels_Rover::get_arming_channel(void) const
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel_Rover::do_aux_function_change_mode(Mode &mode, |
|
|
|
|
const aux_switch_pos_t ch_flag) |
|
|
|
|
const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
rover.set_mode(mode, ModeReason::RC_COMMAND); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// do nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
if (rover.control_mode == &mode) { |
|
|
|
|
rc().reset_mode_switch(); |
|
|
|
|
} |
|
|
|
@ -107,28 +107,28 @@ void RC_Channel_Rover::add_waypoint_for_current_loc()
@@ -107,28 +107,28 @@ void RC_Channel_Rover::add_waypoint_for_current_loc()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ALWAYS); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_ASSIST); |
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
rover.g2.sailboat.set_motor_state(Sailboat::UseMotor::USE_MOTOR_NEVER); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch (ch_option) { |
|
|
|
|
case AUX_FUNC::DO_NOTHING: |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::SAVE_WP: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
// do nothing if in AUTO mode
|
|
|
|
|
if (rover.control_mode == &rover.mode_auto) { |
|
|
|
|
return; |
|
|
|
@ -156,7 +156,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
@@ -156,7 +156,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
|
|
|
|
|
|
|
|
|
|
// learn cruise speed and throttle
|
|
|
|
|
case AUX_FUNC::LEARN_CRUISE: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
rover.cruise_learn_start(); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
@ -235,7 +235,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
@@ -235,7 +235,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
|
|
|
|
|
case AUX_FUNC::SAVE_TRIM: |
|
|
|
|
if (!rover.g2.motors.have_skid_steering() && rover.arming.is_armed() && |
|
|
|
|
(rover.control_mode != &rover.mode_loiter) |
|
|
|
|
&& (rover.control_mode != &rover.mode_hold) && ch_flag == HIGH) { |
|
|
|
|
&& (rover.control_mode != &rover.mode_hold) && ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_steering); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Steering trim saved!"); |
|
|
|
|
} |
|
|
|
|