|
|
|
@ -409,7 +409,7 @@ void RC_Channel::read_mode_switch()
@@ -409,7 +409,7 @@ void RC_Channel::read_mode_switch()
|
|
|
|
|
else if (pulsewidth < 1750) position = 4; |
|
|
|
|
else position = 5; |
|
|
|
|
|
|
|
|
|
if (!debounce_completed(position)) { |
|
|
|
|
if (!debounce_completed((int8_t)position)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -446,7 +446,7 @@ bool RC_Channel::debounce_completed(int8_t position)
@@ -446,7 +446,7 @@ bool RC_Channel::debounce_completed(int8_t position)
|
|
|
|
|
//
|
|
|
|
|
|
|
|
|
|
// init_aux_switch_function - initialize aux functions
|
|
|
|
|
void RC_Channel::init_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
// init channel options
|
|
|
|
|
switch(ch_option) { |
|
|
|
@ -506,12 +506,12 @@ bool RC_Channel::read_aux()
@@ -506,12 +506,12 @@ bool RC_Channel::read_aux()
|
|
|
|
|
// here e.g. RCMAP_ROLL etc once they become options
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
aux_switch_pos_t new_position; |
|
|
|
|
AuxSwitchPos new_position; |
|
|
|
|
if (!read_3pos_switch(new_position)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (!debounce_completed(new_position)) { |
|
|
|
|
if (!debounce_completed((int8_t)new_position)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -521,23 +521,23 @@ bool RC_Channel::read_aux()
@@ -521,23 +521,23 @@ bool RC_Channel::read_aux()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_armdisarm(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_armdisarm(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
// arm or disarm the vehicle
|
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
AP::arming().arm(AP_Arming::Method::AUXSWITCH, true); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
AP::arming().disarm(AP_Arming::Method::AUXSWITCH); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_avoid_adsb(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AP_Avoidance *avoidance = AP::ap_avoidance(); |
|
|
|
|
if (avoidance == nullptr) { |
|
|
|
@ -547,7 +547,7 @@ void RC_Channel::do_aux_function_avoid_adsb(const aux_switch_pos_t ch_flag)
@@ -547,7 +547,7 @@ void RC_Channel::do_aux_function_avoid_adsb(const aux_switch_pos_t ch_flag)
|
|
|
|
|
if (adsb == nullptr) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
// try to enable AP_Avoidance
|
|
|
|
|
if (!adsb->enabled()) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "ADSB not enabled"); |
|
|
|
@ -565,7 +565,7 @@ void RC_Channel::do_aux_function_avoid_adsb(const aux_switch_pos_t ch_flag)
@@ -565,7 +565,7 @@ void RC_Channel::do_aux_function_avoid_adsb(const aux_switch_pos_t ch_flag)
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "ADSB Avoidance Disabled"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_avoid_proximity(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_avoid_proximity(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AC_Avoid *avoid = AP::ac_avoid(); |
|
|
|
|
if (avoid == nullptr) { |
|
|
|
@ -573,30 +573,30 @@ void RC_Channel::do_aux_function_avoid_proximity(const aux_switch_pos_t ch_flag)
@@ -573,30 +573,30 @@ void RC_Channel::do_aux_function_avoid_proximity(const aux_switch_pos_t ch_flag)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
avoid->proximity_avoidance_enable(true); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
avoid->proximity_avoidance_enable(false); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_camera_trigger(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_camera_trigger(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AP_Camera *camera = AP::camera(); |
|
|
|
|
if (camera == nullptr) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
camera->take_picture(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_runcam_control(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
#if HAL_RUNCAM_ENABLED |
|
|
|
|
AP_RunCam *runcam = AP::runcam(); |
|
|
|
@ -605,20 +605,20 @@ void RC_Channel::do_aux_function_runcam_control(const aux_switch_pos_t ch_flag)
@@ -605,20 +605,20 @@ void RC_Channel::do_aux_function_runcam_control(const aux_switch_pos_t ch_flag)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
runcam->start_recording(); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
runcam->osd_option(); |
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
runcam->stop_recording(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_runcam_osd_control(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_runcam_osd_control(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
#if HAL_RUNCAM_ENABLED |
|
|
|
|
AP_RunCam *runcam = AP::runcam(); |
|
|
|
@ -627,11 +627,11 @@ void RC_Channel::do_aux_function_runcam_osd_control(const aux_switch_pos_t ch_fl
@@ -627,11 +627,11 @@ void RC_Channel::do_aux_function_runcam_osd_control(const aux_switch_pos_t ch_fl
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
runcam->enter_osd(); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
runcam->exit_osd(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -639,23 +639,23 @@ void RC_Channel::do_aux_function_runcam_osd_control(const aux_switch_pos_t ch_fl
@@ -639,23 +639,23 @@ void RC_Channel::do_aux_function_runcam_osd_control(const aux_switch_pos_t ch_fl
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// enable or disable the fence
|
|
|
|
|
void RC_Channel::do_aux_function_fence(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AC_Fence *fence = AP::fence(); |
|
|
|
|
if (fence == nullptr) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
fence->enable(ch_flag == HIGH); |
|
|
|
|
fence->enable(ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_clear_wp(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_clear_wp(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AP_Mission *mission = AP::mission(); |
|
|
|
|
if (mission == nullptr) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
mission->clear(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -669,7 +669,7 @@ void RC_Channel::do_aux_function_relay(const uint8_t relay, bool val)
@@ -669,7 +669,7 @@ void RC_Channel::do_aux_function_relay(const uint8_t relay, bool val)
|
|
|
|
|
servorelayevents->do_set_relay(relay, val); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_sprayer(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_sprayer(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
#if HAL_SPRAYER_ENABLED |
|
|
|
|
AC_Sprayer *sprayer = AP::sprayer(); |
|
|
|
@ -677,13 +677,13 @@ void RC_Channel::do_aux_function_sprayer(const aux_switch_pos_t ch_flag)
@@ -677,13 +677,13 @@ void RC_Channel::do_aux_function_sprayer(const aux_switch_pos_t ch_flag)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
sprayer->run(ch_flag == HIGH); |
|
|
|
|
sprayer->run(ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
// if we are disarmed the pilot must want to test the pump
|
|
|
|
|
sprayer->test_pump((ch_flag == HIGH) && !hal.util->get_soft_armed()); |
|
|
|
|
sprayer->test_pump((ch_flag == AuxSwitchPos::HIGH) && !hal.util->get_soft_armed()); |
|
|
|
|
#endif // HAL_SPRAYER_ENABLED
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_gripper(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_gripper(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
AP_Gripper *gripper = AP::gripper(); |
|
|
|
|
if (gripper == nullptr) { |
|
|
|
@ -691,53 +691,53 @@ void RC_Channel::do_aux_function_gripper(const aux_switch_pos_t ch_flag)
@@ -691,53 +691,53 @@ void RC_Channel::do_aux_function_gripper(const aux_switch_pos_t ch_flag)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
switch(ch_flag) { |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
gripper->release(); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
gripper->grab(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_lost_vehicle_sound(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_lost_vehicle_sound(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
AP_Notify::flags.vehicle_lost = true; |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
AP_Notify::flags.vehicle_lost = false; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_rc_override_enable(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_rc_override_enable(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: { |
|
|
|
|
case AuxSwitchPos::HIGH: { |
|
|
|
|
rc().set_gcs_overrides_enabled(true); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: { |
|
|
|
|
case AuxSwitchPos::LOW: { |
|
|
|
|
rc().set_gcs_overrides_enabled(false); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function_mission_reset(const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
if (ch_flag != HIGH) { |
|
|
|
|
if (ch_flag != AuxSwitchPos::HIGH) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
AP_Mission *mission = AP::mission(); |
|
|
|
@ -747,7 +747,7 @@ void RC_Channel::do_aux_function_mission_reset(const aux_switch_pos_t ch_flag)
@@ -747,7 +747,7 @@ void RC_Channel::do_aux_function_mission_reset(const aux_switch_pos_t ch_flag)
|
|
|
|
|
mission->reset(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) |
|
|
|
|
void RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) |
|
|
|
|
{ |
|
|
|
|
switch(ch_option) { |
|
|
|
|
case AUX_FUNC::CAMERA_TRIGGER: |
|
|
|
@ -772,22 +772,22 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -772,22 +772,22 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::RELAY: |
|
|
|
|
do_aux_function_relay(0, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(0, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::RELAY2: |
|
|
|
|
do_aux_function_relay(1, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(1, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::RELAY3: |
|
|
|
|
do_aux_function_relay(2, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(2, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::RELAY4: |
|
|
|
|
do_aux_function_relay(3, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(3, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::RELAY5: |
|
|
|
|
do_aux_function_relay(4, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(4, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
case AUX_FUNC::RELAY6: |
|
|
|
|
do_aux_function_relay(5, ch_flag == HIGH); |
|
|
|
|
do_aux_function_relay(5, ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::RUNCAM_CONTROL: |
|
|
|
@ -822,13 +822,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -822,13 +822,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::DISARM: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
AP::arming().disarm(AP_Arming::Method::AUXSWITCH); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::COMPASS_LEARN: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
Compass &compass = AP::compass(); |
|
|
|
|
compass.set_learn_type(Compass::LEARN_INFLIGHT, false); |
|
|
|
|
} |
|
|
|
@ -840,13 +840,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -840,13 +840,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
lg->set_position(AP_LandingGear::LandingGear_Deploy); |
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
lg->set_position(AP_LandingGear::LandingGear_Retract); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -854,12 +854,12 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -854,12 +854,12 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::GPS_DISABLE: |
|
|
|
|
AP::gps().force_disable(ch_flag == HIGH); |
|
|
|
|
AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::MOTOR_ESTOP: |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case HIGH: { |
|
|
|
|
case AuxSwitchPos::HIGH: { |
|
|
|
|
SRV_Channels::set_emergency_stop(true); |
|
|
|
|
|
|
|
|
|
// log E-stop
|
|
|
|
@ -869,10 +869,10 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -869,10 +869,10 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case LOW: { |
|
|
|
|
case AuxSwitchPos::LOW: { |
|
|
|
|
SRV_Channels::set_emergency_stop(false); |
|
|
|
|
|
|
|
|
|
// log E-stop cleared
|
|
|
|
@ -887,7 +887,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -887,7 +887,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
|
|
|
|
|
case AUX_FUNC::VISODOM_CALIBRATE: |
|
|
|
|
#if HAL_VISUALODOM_ENABLED |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
AP_VisualOdom *visual_odom = AP::visualodom(); |
|
|
|
|
if (visual_odom != nullptr) { |
|
|
|
|
visual_odom->align_sensor_to_vehicle(); |
|
|
|
@ -898,7 +898,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -898,7 +898,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
|
|
|
|
|
#if !HAL_MINIMIZE_FEATURES |
|
|
|
|
case AUX_FUNC::KILL_IMU1: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
AP::ins().kill_imu(0, true); |
|
|
|
|
} else { |
|
|
|
|
AP::ins().kill_imu(0, false); |
|
|
|
@ -906,7 +906,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -906,7 +906,7 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AUX_FUNC::KILL_IMU2: |
|
|
|
|
if (ch_flag == HIGH) { |
|
|
|
|
if (ch_flag == AuxSwitchPos::HIGH) { |
|
|
|
|
AP::ins().kill_imu(1, true); |
|
|
|
|
} else { |
|
|
|
|
AP::ins().kill_imu(1, false); |
|
|
|
@ -921,13 +921,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -921,13 +921,13 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
switch (ch_flag) { |
|
|
|
|
case LOW: |
|
|
|
|
case AuxSwitchPos::LOW: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case MIDDLE: |
|
|
|
|
case AuxSwitchPos::MIDDLE: |
|
|
|
|
// nothing
|
|
|
|
|
break; |
|
|
|
|
case HIGH: |
|
|
|
|
case AuxSwitchPos::HIGH: |
|
|
|
|
camera->cam_mode_toggle(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
@ -952,15 +952,15 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
@@ -952,15 +952,15 @@ void RC_Channel::do_aux_function(const aux_func_t ch_option, const aux_switch_po
|
|
|
|
|
|
|
|
|
|
void RC_Channel::init_aux() |
|
|
|
|
{ |
|
|
|
|
aux_switch_pos_t position; |
|
|
|
|
AuxSwitchPos position; |
|
|
|
|
if (!read_3pos_switch(position)) { |
|
|
|
|
position = aux_switch_pos_t::LOW; |
|
|
|
|
position = AuxSwitchPos::LOW; |
|
|
|
|
} |
|
|
|
|
init_aux_function((aux_func_t)option.get(), position); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// read_3pos_switch
|
|
|
|
|
bool RC_Channel::read_3pos_switch(RC_Channel::aux_switch_pos_t &ret) const |
|
|
|
|
bool RC_Channel::read_3pos_switch(RC_Channel::AuxSwitchPos &ret) const |
|
|
|
|
{ |
|
|
|
|
const uint16_t in = get_radio_in(); |
|
|
|
|
if (in <= 900 or in >= 2200) { |
|
|
|
@ -971,20 +971,20 @@ bool RC_Channel::read_3pos_switch(RC_Channel::aux_switch_pos_t &ret) const
@@ -971,20 +971,20 @@ bool RC_Channel::read_3pos_switch(RC_Channel::aux_switch_pos_t &ret) const
|
|
|
|
|
bool switch_reversed = reversed && rc().switch_reverse_allowed(); |
|
|
|
|
|
|
|
|
|
if (in < AUX_PWM_TRIGGER_LOW) { |
|
|
|
|
ret = switch_reversed ? HIGH : LOW; |
|
|
|
|
ret = switch_reversed ? AuxSwitchPos::HIGH : AuxSwitchPos::LOW; |
|
|
|
|
} else if (in > AUX_PWM_TRIGGER_HIGH) { |
|
|
|
|
ret = switch_reversed ? LOW : HIGH; |
|
|
|
|
ret = switch_reversed ? AuxSwitchPos::LOW : AuxSwitchPos::HIGH; |
|
|
|
|
} else { |
|
|
|
|
ret = MIDDLE; |
|
|
|
|
ret = AuxSwitchPos::MIDDLE; |
|
|
|
|
} |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// return switch position value as LOW, MIDDLE, HIGH
|
|
|
|
|
// if reading the switch fails then it returns LOW
|
|
|
|
|
RC_Channel::aux_switch_pos_t RC_Channel::get_aux_switch_pos() const |
|
|
|
|
RC_Channel::AuxSwitchPos RC_Channel::get_aux_switch_pos() const |
|
|
|
|
{ |
|
|
|
|
aux_switch_pos_t position = aux_switch_pos_t::LOW; |
|
|
|
|
AuxSwitchPos position = AuxSwitchPos::LOW; |
|
|
|
|
UNUSED_RESULT(read_3pos_switch(position)); |
|
|
|
|
|
|
|
|
|
return position; |
|
|
|
@ -992,10 +992,10 @@ RC_Channel::aux_switch_pos_t RC_Channel::get_aux_switch_pos() const
@@ -992,10 +992,10 @@ RC_Channel::aux_switch_pos_t RC_Channel::get_aux_switch_pos() const
|
|
|
|
|
|
|
|
|
|
// return switch position value as LOW, MIDDLE, HIGH
|
|
|
|
|
// if reading the switch fails then it returns LOW
|
|
|
|
|
RC_Channel::aux_switch_pos_t RC_Channels::get_channel_pos(const uint8_t rcmapchan) const |
|
|
|
|
RC_Channel::AuxSwitchPos RC_Channels::get_channel_pos(const uint8_t rcmapchan) const |
|
|
|
|
{ |
|
|
|
|
const RC_Channel* chan = rc().channel(rcmapchan-1); |
|
|
|
|
return chan != nullptr ? chan->get_aux_switch_pos() : RC_Channel::aux_switch_pos_t::LOW; |
|
|
|
|
return chan != nullptr ? chan->get_aux_switch_pos() : RC_Channel::AuxSwitchPos::LOW; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
RC_Channel *RC_Channels::find_channel_for_option(const RC_Channel::aux_func_t option) |
|
|
|
|