Rover: add arm/disarm to ch7 switch

This commit is contained in:
Randy Mackay 2017-12-01 12:15:50 +09:00
parent 8eb58b266a
commit b4a779aec3
3 changed files with 12 additions and 2 deletions

View File

@ -129,7 +129,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: CH7_OPTION
// @DisplayName: Channel 7 option
// @Description: What to do use channel 7 for
// @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed
// @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm
// @User: Standard
GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION),

View File

@ -189,6 +189,15 @@ void Rover::read_aux_switch()
cruise_learn_complete();
}
break;
// arm or disarm the motors
case CH7_ARM_DISARM:
if (aux_ch7 == AUX_SWITCH_HIGH) {
arm_motors(AP_Arming::RUDDER);
} else if (aux_ch7 == AUX_SWITCH_LOW) {
disarm_motors();
}
break;
}
}

View File

@ -17,7 +17,8 @@
enum ch7_option {
CH7_DO_NOTHING = 0,
CH7_SAVE_WP = 1,
CH7_LEARN_CRUISE = 2
CH7_LEARN_CRUISE = 2,
CH7_ARM_DISARM = 3
};
// HIL enumerations