/// TODO: Be able to tell if the RC has become disconnected during flight bool check_failsafe(void) { for(int8_t i = 0; i<get_param_int(PARAM_RC_NUM_CHANNELS); i++) { if(pwmRead(i) < 900 || pwmRead(i) > 2100) { if(_armed_state == ARMED || _armed_state == DISARMED) { _armed_state = FAILSAFE_DISARMED; } // blink LED static uint8_t count = 0; if(count > 25) { LED1_TOGGLE; count = 0; } count++; return true; } } // we got a valid RC measurement for all channels if(_armed_state == FAILSAFE_ARMED || _armed_state == FAILSAFE_DISARMED) { // return to appropriate mode _armed_state = (_armed_state == FAILSAFE_ARMED) ? ARMED : DISARMED; } return false; }
uint16_t pwmReadRawRC(uint8_t chan) { uint16_t data; data = pwmRead(cfg.rcmap[chan]); if (data < 750 || data > 2250) data = cfg.midrc; return data; }
static void mavlink_send_rc_raw(void) { mavlink_msg_rc_channels_send(MAVLINK_COMM_0, millis(), 0, pwmRead(0), pwmRead(1), pwmRead(2), pwmRead(3), pwmRead(4), pwmRead(5), pwmRead(6), pwmRead(7), 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0); }
uint16_t pwmReadRawRC(uint8_t chan) { return pwmRead(mcfg.rcmap[chan]); }
void processFlightCommands(void) { uint8_t channel; if ( rcActive == true ) { // Read receiver commands for (channel = 0; channel < 8; channel++) rxCommand[channel] = (float)pwmRead(systemConfig.rcMap[channel]); rxCommand[ROLL] -= systemConfig.midCommand; // Roll Range -1000:1000 rxCommand[PITCH] -= systemConfig.midCommand; // Pitch Range -1000:1000 rxCommand[YAW] -= systemConfig.midCommand; // Yaw Range -1000:1000 rxCommand[THROTTLE] -= systemConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000 rxCommand[AUX1] -= systemConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000 rxCommand[AUX2] -= systemConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000 rxCommand[AUX3] -= systemConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000 rxCommand[AUX4] -= systemConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000 } // Set past command in detent values for (channel = 0; channel < 3; channel++) previousCommandInDetent[channel] = commandInDetent[channel]; // Apply deadbands and set detent discretes' for (channel = 0; channel < 3; channel++) { if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND)) { rxCommand[channel] = 0; commandInDetent[channel] = true; } else { commandInDetent[channel] = false; if (rxCommand[channel] > 0) { rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE; } else { rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE; } } } /////////////////////////////////// // Check for low throttle if ( rxCommand[THROTTLE] < systemConfig.minCheck ) { // Check for disarm command ( low throttle, left yaw ), will disarm immediately if ( (rxCommand[YAW] < (systemConfig.minCheck - MIDCOMMAND)) && (armed == true) ) { armed = false; // Zero PID integrators when disarmed zeroIntegralError(); zeroLastError(); } // Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll ) if ( (rxCommand[YAW ] < (systemConfig.minCheck - MIDCOMMAND)) && (rxCommand[ROLL ] > (systemConfig.maxCheck - MIDCOMMAND)) && (rxCommand[PITCH] < (systemConfig.minCheck - MIDCOMMAND)) ) { computeGyroRTBias(); pulseMotors(3); } // Check for arm command ( low throttle, right yaw), must be present for 1 sec before arming if ( (rxCommand[YAW] > (systemConfig.maxCheck - MIDCOMMAND) ) && (armed == false) ) { armingTimer++; if ( armingTimer > 50 ) { zeroIntegralError(); armed = true; armingTimer = 0; } } else { armingTimer = 0; } } // Check for armed true and throttle command > minThrottle if ( (armed == true) && (rxCommand[THROTTLE] > systemConfig.minThrottle) ) holdIntegrators = false; else holdIntegrators = true; // Check AUX1 for rate or attitude mode if ( rxCommand[AUX1] > MIDCOMMAND ) { flightMode = ATTITUDE; } else { flightMode = RATE; } }
bool check_mode(uint32_t now) { static uint32_t prev_time = 0; static uint32_t time_sticks_have_been_in_arming_position = 0; // see it has been at least 20 ms uint32_t dt = now-prev_time; if (dt < 20000) { return false; } // if it has, then do stuff prev_time = now; // check for failsafe mode if(check_failsafe()) { return true; } else { // check for arming switch if (get_param_int(PARAM_ARM_STICKS)) { if (_armed_state == DISARMED) { // if left stick is down and to the right if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) + get_param_int(PARAM_ARM_THRESHOLD) && pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) > (get_param_int(PARAM_RC_Z_CENTER) + get_param_int(PARAM_RC_Z_RANGE)/2) - get_param_int(PARAM_ARM_THRESHOLD)) { time_sticks_have_been_in_arming_position += dt; } else { time_sticks_have_been_in_arming_position = 0; } if (time_sticks_have_been_in_arming_position > 500000) { arm(); time_sticks_have_been_in_arming_position = 0; } } else // _armed_state is ARMED { // if left stick is down and to the left if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) + get_param_int(PARAM_ARM_THRESHOLD) && pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) < (get_param_int(PARAM_RC_Z_CENTER) - get_param_int(PARAM_RC_Z_RANGE)/2) + get_param_int(PARAM_ARM_THRESHOLD)) { time_sticks_have_been_in_arming_position += dt; } else { time_sticks_have_been_in_arming_position = 0; } if (time_sticks_have_been_in_arming_position > 500000) { disarm(); time_sticks_have_been_in_arming_position = 0; } } } else { if (rc_switch(get_param_int(PARAM_ARM_CHANNEL))) { arm(); } else { disarm(); } } } return true; }