/* check for driver input on rudder/steering stick for arming/disarming */ void Rover::rudder_arm_disarm_check() { // In Rover we need to check that its set to the throttle trim and within the DZ // if throttle is not within trim dz, then pilot cannot rudder arm/disarm if (!channel_throttle->in_trim_dz()) { rudder_arm_timer = 0; return; } // if not in a manual throttle mode then disallow rudder arming/disarming if (control_mode->auto_throttle()) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_steer->get_control_in() > 4000) { const uint32_t now = millis(); 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() & !g2.motors.have_skid_steering()) { // 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) { const uint32_t now = millis(); 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; } } }
/* check for driver input on rudder/steering stick for arming/disarming */ void Rover::rudder_arm_disarm_check() { // In Rover we need to check that its set to the throttle trim and within the DZ // if throttle is not within trim dz, then pilot cannot rudder arm/disarm if (!channel_throttle->in_trim_dz()) { rudder_arm_timer = 0; return; } // check if arming/disarming allowed from this mode if (!control_mode->allows_arming_from_transmitter()) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_steer->get_control_in() > 4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < ARM_DELAY_MS) { 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()) { // when armed and motor not active (not moving), full left rudder starts disarming counter if (channel_steer->get_control_in() < -4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < ARM_DELAY_MS) { 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; } } }
/* update AP_Stats */ void Rover::stats_update(void) { g2.stats.set_flying(motor_active()); g2.stats.update(); }