void Plane::read_control_switch() { static bool switch_debouncer; uint8_t switchPosition = readSwitch(); // If switchPosition = 255 this indicates that the mode control channel input was out of range // If we get this value we do not want to change modes. if(switchPosition == 255) return; if (failsafe.ch3_failsafe || failsafe.ch3_counter > 0) { // when we are in ch3_failsafe mode then RC input is not // working, and we need to ignore the mode switch channel return; } if (millis() - failsafe.last_valid_rc_ms > 100) { // only use signals that are less than 0.1s old. return; } // we look for changes in the switch position. If the // RST_SWITCH_CH parameter is set, then it is a switch that can be // used to force re-reading of the control switch. This is useful // when returning to the previous mode after a failsafe or fence // breach. This channel is best used on a momentary switch (such // as a spring loaded trainer switch). if (oldSwitchPosition != switchPosition || (g.reset_switch_chan != 0 && hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { if (switch_debouncer == false) { // this ensures that mode switches only happen if the // switch changes for 2 reads. This prevents momentary // spikes in the mode control channel from causing a mode // switch switch_debouncer = true; return; } set_mode((enum FlightMode)(flight_modes[switchPosition].get())); oldSwitchPosition = switchPosition; } if (g.reset_mission_chan != 0 && hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) { mission.start(); prev_WP_loc = current_loc; } switch_debouncer = false; if (g.inverted_flight_ch != 0) { // if the user has configured an inverted flight channel, then // fly upside down when that channel goes above INVERTED_FLIGHT_PWM inverted_flight = (control_mode != MANUAL && hal.rcin->read(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM); } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 if (g.override_channel > 0) { // if the user has configured an override channel then check it bool override = (hal.rcin->read(g.override_channel-1) >= PX4IO_OVERRIDE_PWM); if (override && !px4io_override_enabled) { // we only update the mixer if we are not armed. This is // important as otherwise we will need to temporarily // disarm to change the mixer if (hal.util->get_soft_armed() || setup_failsafe_mixing()) { px4io_override_enabled = true; // disable output channels to force PX4IO override gcs_send_text_P(SEVERITY_LOW, PSTR("PX4IO Override enabled")); } else { // we'll try again next loop. The PX4IO code sometimes // rejects a mixer, probably due to it being busy in // some way? gcs_send_text_P(SEVERITY_LOW, PSTR("PX4IO Override enable failed")); } } else if (!override && px4io_override_enabled) {
void Copter::do_erase_logs(void) { gcs_send_text_P(SEVERITY_HIGH, PSTR("Erasing logs\n")); DataFlash.EraseAll(); gcs_send_text_P(SEVERITY_HIGH, PSTR("Log erase complete\n")); }
void Copter::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for // now is to delay for 1 second. Something more elegant may be // added later delay(1000); } // initialise serial port serial_manager.init_console(); cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n"), hal.util->available_memory()); // // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) // report_version(); // load parameters from EEPROM load_parameters(); BoardConfig.init(); // initialise serial port serial_manager.init(); // init EPM cargo gripper #if EPM_ENABLED == ENABLED epm.init(); #endif // initialise notify system // disable external leds if epm is enabled because of pin conflict on the APM notify.init(true); // initialise battery monitor battery.init(); rssi_analog_source = hal.analogin->channel(g.rssi_pin); barometer.init(); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); // init the GCS connected to the console gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); // init telemetry port gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); #if MAVLINK_COMM_NUM_BUFFERS > 2 // setup serial port for telem2 gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); #endif #if MAVLINK_COMM_NUM_BUFFERS > 3 // setup serial port for fourth telemetry port (not used by default) gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); #endif #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky frsky_telemetry.init(serial_manager); #endif // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs // initialise which outputs Servo and Relay events can use ServoRelayEvents.set_channel_mask(~motors.get_motor_mask()); relay.init(); /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // Do GPS init gps.init(&DataFlash, serial_manager); if(g.compass_enabled) init_compass(); #if OPTFLOW == ENABLED // make optflow available to AHRS ahrs.set_optflow(&optflow); #endif // initialise attitude and position controllers attitude_control.set_dt(MAIN_LOOP_SECONDS); pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor init_optflow(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(serial_manager); #endif #ifdef USERHOOK_INIT USERHOOK_INIT #endif #if CLI_ENABLED == ENABLED if (g.cli_enabled) { const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n"); cliSerial->println_P(msg); if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) { gcs[1].get_uart()->println_P(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { gcs[2].get_uart()->println_P(msg); } } #endif // CLI_ENABLED #if HIL_MODE != HIL_MODE_DISABLED while (barometer.get_last_update() == 0) { // the barometer begins updating when we get the first // HIL_STATE message gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message")); delay(1000); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(true); // initialise sonar #if CONFIG_SONAR == ENABLED init_sonar(); #endif // initialise mission library mission.init(); // initialise the flight mode and aux switch // --------------------------- reset_control_switch(); init_aux_switches(); #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif startup_ground(true); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly serial_manager.set_blocking_writes_all(false); // enable CPU failsafe failsafe_enable(); ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); cliSerial->print_P(PSTR("\nReady to FLY ")); // flag that initialisation has completed ap.initialised = true; }
void Copter::failsafe_battery_event(void) { // return immediately if low battery event has already been triggered if (failsafe.battery) { return; } // failsafe check if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) { switch(control_mode) { case STABILIZE: case ACRO: // if throttle is zero OR vehicle is landed disarm motors if (ap.throttle_zero || ap.land_complete) { init_disarm_motors(); }else{ // set mode to RTL or LAND if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); }else{ set_mode_land_with_pause(); } } break; case AUTO: // if mission has not started AND vehicle is landed, disarm motors if (!ap.auto_armed && ap.land_complete) { init_disarm_motors(); // set mode to RTL or LAND } else if (home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); } else { set_mode_land_with_pause(); } break; default: // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold // if landed disarm if (ap.land_complete) { init_disarm_motors(); // set mode to RTL or LAND } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) { // switch to RTL or if that fails, LAND set_mode_RTL_or_land_with_pause(); } else { set_mode_land_with_pause(); } break; } } // set the low battery flag set_failsafe_battery(true); // warn the ground station and log to dataflash gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED); }
bool AP_Arming::ins_checks(bool report) { if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { const AP_InertialSensor &ins = ahrs.get_ins(); if (! ins.get_gyro_health_all()) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: gyros not healthy!")); } return false; } if (!skip_gyro_cal && ! ins.gyro_calibrated_ok_all()) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: gyros not calibrated!")); } return false; } if (! ins.get_accel_health_all()) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: accels not healthy!")); } return false; } if (!ahrs.healthy()) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: AHRS not healthy!")); } return false; } if (!ins.calibrated()) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: 3D accel cal needed")); } return false; } #if INS_MAX_INSTANCES > 1 // check all accelerometers point in roughly same direction if (ins.get_accel_count() > 1) { const Vector3f &prime_accel_vec = ins.get_accel(); for(uint8_t i=0; i<ins.get_accel_count(); i++) { // get next accel vector const Vector3f &accel_vec = ins.get_accel(i); Vector3f vec_diff = accel_vec - prime_accel_vec; // allow for up to 0.3 m/s/s difference if (vec_diff.length() > 0.3f) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers")); } return false; } } } // check all gyros are giving consistent readings if (ins.get_gyro_count() > 1) { const Vector3f &prime_gyro_vec = ins.get_gyro(); for(uint8_t i=0; i<ins.get_gyro_count(); i++) { // get next gyro vector const Vector3f &gyro_vec = ins.get_gyro(i); Vector3f vec_diff = gyro_vec - prime_gyro_vec; // allow for up to 5 degrees/s difference if (vec_diff.length() > radians(5)) { if (report) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros")); } return false; } } } #endif } return true; }
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete { switch(cmd.id) { case MAV_CMD_NAV_TAKEOFF: return verify_takeoff(); case MAV_CMD_NAV_LAND: return verify_land(); case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); case MAV_CMD_NAV_LOITER_UNLIM: return verify_loiter_unlim(); case MAV_CMD_NAV_LOITER_TURNS: return verify_loiter_turns(); case MAV_CMD_NAV_LOITER_TIME: return verify_loiter_time(); case MAV_CMD_NAV_LOITER_TO_ALT: return verify_loiter_to_alt(); case MAV_CMD_NAV_RETURN_TO_LAUNCH: return verify_RTL(); case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: return verify_continue_and_change_alt(); case MAV_CMD_NAV_ALTITUDE_WAIT: return verify_altitude_wait(cmd); // Conditional commands case MAV_CMD_CONDITION_DELAY: return verify_wait_delay(); case MAV_CMD_CONDITION_DISTANCE: return verify_within_distance(); case MAV_CMD_CONDITION_CHANGE_ALT: return verify_change_alt(); // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_CONTROL_VIDEO: case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_NAV_ROI: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_INVERTED_FLIGHT: case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_FENCE_ENABLE: case MAV_CMD_DO_AUTOTUNE_ENABLE: return true; default: // error message if (AP_Mission::is_nav_cmd(cmd)) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("verify_nav: Invalid or no current Nav cmd")); }else{ gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("verify_conditon: Invalid or no current Condition cmd")); } // return true so that we do not get stuck at this command return true; } }
bool Plane::verify_takeoff() { if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { const float min_gps_speed = 5; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.ground_speed() > min_gps_speed) { auto_state.takeoff_speed_time_ms = millis(); } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { // once we reach sufficient speed for good GPS course // estimation we save our current GPS ground course // corrected for summed yaw to set the take off // course. This keeps wings level until we are ready to // rotate, and also allows us to cope with arbitary // compass errors for auto takeoff float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01f)) - steer_state.locked_course_err; takeoff_course = wrap_PI(takeoff_course); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"), steer_state.hold_course_cd, (double)gps.ground_speed(), (double)degrees(steer_state.locked_course_err)); } } if (steer_state.hold_course_cd != -1) { // call navigation controller for heading hold nav_controller->update_heading_hold(steer_state.hold_course_cd); } else { nav_controller->update_level_flight(); } // see if we have reached takeoff altitude int32_t relative_alt_cm = adjusted_relative_altitude_cm(); if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) { gcs_send_text_fmt(PSTR("Takeoff complete at %.2fm"), (double)(relative_alt_cm*0.01f)); steer_state.hold_course_cd = -1; auto_state.takeoff_complete = true; next_WP_loc = prev_WP_loc = current_loc; #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable > 0) { if (! geofence_set_enabled(true, AUTO_TOGGLED)) { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Enable fence failed (cannot autoenable")); } else { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Fence enabled. (autoenabled)")); } } #endif // don't cross-track on completion of takeoff, as otherwise we // can end up doing too sharp a turn auto_state.next_wp_no_crosstrack = true; return true; } else { return false; } }
// performs pre_arm gps related checks and returns true if passed bool Copter::pre_arm_gps_checks(bool display_failure) { // always check if inertial nav has started and is ready if(!ahrs.get_NavEKF().healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Waiting for Nav Checks")); } return false; } // check if flight mode requires GPS bool gps_required = mode_requires_GPS(control_mode); #if AC_FENCE == ENABLED // if circular fence is enabled we need GPS if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { gps_required = true; } #endif // return true if GPS is not required if (!gps_required) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // ensure GPS is ok if (!position_ok()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Need 3D Fix")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // check EKF compass variance is below failsafe threshold float vel_variance, pos_variance, hgt_variance, tas_variance; Vector3f mag_variance; Vector2f offset; ahrs.get_NavEKF().getVariances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); if (mag_variance.length() >= g.fs_ekf_thresh) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: EKF compass variance")); } return false; } // check home and EKF origin are not too far if (far_from_EKF_origin(ahrs.get_home())) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: EKF-home variance")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // return true immediately if gps check is disabled if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { AP_Notify::flags.pre_arm_gps_check = true; return true; } // warn about hdop separately - to prevent user confusion with no gps lock if (gps.get_hdop() > g.gps_hdop_good) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: High GPS HDOP")); } AP_Notify::flags.pre_arm_gps_check = false; return false; } // if we got here all must be ok AP_Notify::flags.pre_arm_gps_check = true; return true; }
// arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm // has side-effect that logging is started bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) { #if LOGGING_ENABLED == ENABLED // start dataflash start_logging(); #endif // check accels and gyro are healthy if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { if(!ins.get_accel_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Accelerometers not healthy")); } return false; } if(!ins.get_gyro_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyros not healthy")); } return false; } } // always check if inertial nav has started and is ready if(!ahrs.healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Waiting for Nav Checks")); } return false; } // always check if the current mode allows arming if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Mode not armable")); } return false; } // always check gps if (!pre_arm_gps_checks(display_failure)) { return false; } // heli specific arming check #if FRAME_CONFIG == HELI_FRAME // check if rotor is spinning on heli because this could disrupt gyro calibration if (!motors.allow_arming()){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Rotor is Spinning")); } return false; } #endif // HELI_FRAME // succeed if arming checks are disabled if (g.arming_check == ARMING_CHECK_NONE) { return true; } // baro checks if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { // baro health check if (!barometer.all_healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Barometer not healthy")); } return false; } // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height // that may differ from the baro height due to baro drift. nav_filter_status filt_status = inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Altitude disparity")); } return false; } } #if AC_FENCE == ENABLED // check vehicle is within fence if(!fence.pre_arm_check()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: check fence")); } return false; } #endif // check lean angle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Leaning")); } return false; } } // check battery voltage if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Check Battery")); } return false; } } // check throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective below Failsafe")); #else gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle below Failsafe")); #endif } return false; } // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && control_mode == GUIDED)) { // above top of deadband is too always high if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high")); #else gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high")); #endif } return false; } // in manual modes throttle must be at zero if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Collective too high")); #else gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Throttle too high")); #endif } return false; } } } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Safety Switch")); } return false; } // if we've gotten this far all is ok return true; }
// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { // arming marker // Flag used to track if we have armed the motors the first time. // This is used to decide if we should run the ground_start routine // which calibrates the IMU static bool did_ground_start = false; static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // run pre-arm-checks and display failures if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { update_notify(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("ARMING MOTORS")); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (ap.home_state == HOME_UNSET) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.get_NavEKF().resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ap.home_state == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing(); if(did_ground_start == false) { startup_ground(true); // final check that gyros calibrated successfully if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Gyro calibration failed")); AP_Notify::flags.armed = false; failsafe_enable(); in_arm_motors = false; return false; } did_ground_start = true; } #if FRAME_CONFIG == HELI_FRAME // helicopters are always using motor interlock set_using_interlock(true); #else // check if we are using motor interlock control on an aux switch set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); #endif // if we are using motor interlock switch and it's enabled, fail to arm if (ap.using_interlock && motors.get_interlock()){ gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Interlock Enabled")); AP_Notify::flags.armed = false; in_arm_motors = false; return false; } // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Emergency Stopped")); AP_Notify::flags.armed = false; in_arm_motors = false; return false; } // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // short delay to allow reading of rc inputs delay(30); // enable output to motors enable_motor_output(); // finally actually arm the motors motors.armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming perf_ignore_this_loop(); // flag exiting this function in_arm_motors = false; // return success return true; }
// perform pre-arm checks and set ap.pre_arm_check flag // return true if the checks pass successfully bool Copter::pre_arm_checks(bool display_failure) { // exit immediately if already armed if (motors.armed()) { return true; } // check if motor interlock and Emergency Stop aux switches are used // at the same time. This cannot be allowed. if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Interlock/E-Stop Conflict")); } return false; } // check if motor interlock aux switch is in use // if it is, switch needs to be in disabled position to arm // otherwise exit immediately. This check to be repeated, // as state can change at any time. set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); if (ap.using_interlock && motors.get_interlock()){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Interlock Enabled")); } return false; } // if we are using Motor Emergency Stop aux switch, check it is not enabled // and warn if it is if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Emergency Stopped")); } return false; } // exit immediately if we've already successfully performed the pre-arm check if (ap.pre_arm_check) { // run gps checks because results may change and affect LED colour // no need to display failures because arm_checks will do that if the pilot tries to arm pre_arm_gps_checks(false); return true; } // succeed if pre arm checks are disabled if(g.arming_check == ARMING_CHECK_NONE) { set_pre_arm_check(true); set_pre_arm_rc_check(true); return true; } // pre-arm rc checks a prerequisite pre_arm_rc_checks(); if(!ap.pre_arm_rc_check) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: RC not calibrated")); } return false; } // check Baro if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { // barometer health check if(!barometer.all_healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Barometer not healthy")); } return false; } // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height // that may differ from the baro height due to baro drift. nav_filter_status filt_status = inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref) { if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Altitude disparity")); } return false; } } } // check Compass if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { // check the primary compass is healthy if(!compass.healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass not healthy")); } return false; } // check compass learning is on or offsets have been set if(!compass.configured()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass not calibrated")); } return false; } // check for unreasonable compass offsets Vector3f offsets = compass.get_offsets(); if(offsets.length() > COMPASS_OFFSETS_MAX) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Compass offsets too high")); } return false; } // check for unreasonable mag field length float mag_field = compass.get_field().length(); if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check mag field")); } return false; } #if COMPASS_MAX_INSTANCES > 1 // check all compasses point in roughly same direction if (compass.get_count() > 1) { Vector3f prime_mag_vec = compass.get_field(); prime_mag_vec.normalize(); for(uint8_t i=0; i<compass.get_count(); i++) { // get next compass Vector3f mag_vec = compass.get_field(i); mag_vec.normalize(); Vector3f vec_diff = mag_vec - prime_mag_vec; if (compass.use_for_yaw(i) && vec_diff.length() > COMPASS_ACCEPTABLE_VECTOR_DIFF) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses")); } return false; } } } #endif } // check GPS if (!pre_arm_gps_checks(display_failure)) { return false; } #if AC_FENCE == ENABLED // check fence is initialised if(!fence.pre_arm_check()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: check fence")); } return false; } #endif // check INS if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { // check accelerometers have been calibrated if(!ins.accel_calibrated_ok_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accels not calibrated")); } return false; } // check accels are healthy if(!ins.get_accel_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accelerometers not healthy")); } return false; } #if INS_MAX_INSTANCES > 1 // check all accelerometers point in roughly same direction if (ins.get_accel_count() > 1) { const Vector3f &prime_accel_vec = ins.get_accel(); for(uint8_t i=0; i<ins.get_accel_count(); i++) { // get next accel vector const Vector3f &accel_vec = ins.get_accel(i); Vector3f vec_diff = accel_vec - prime_accel_vec; float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF; if (i >= 2) { /* for boards with 3 IMUs we only use the first two in the EKF. Allow for larger accel discrepancy for IMU3 as it may be running at a different temperature */ threshold *= 2; } if (vec_diff.length() > threshold) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Accelerometers")); } return false; } } } #endif // check gyros are healthy if(!ins.get_gyro_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Gyros not healthy")); } return false; } #if INS_MAX_INSTANCES > 1 // check all gyros are consistent if (ins.get_gyro_count() > 1) { for(uint8_t i=0; i<ins.get_gyro_count(); i++) { // get rotation rate difference between gyro #i and primary gyro Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro(); if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Gyros")); } return false; } } } #endif } #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 // check board voltage if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if(hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Board Voltage")); } return false; } } #endif #endif // check battery voltage if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Battery")); } return false; } } // check various parameter values if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { // ensure ch7 and ch8 have different functions if (check_duplicate_auxsw()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Duplicate Aux Switch Options")); } return false; } // failsafe parameter checks if (g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check FS_THR_VALUE")); } return false; } } // lean angle parameter check if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check ANGLE_MAX")); } return false; } // acro balance parameter check if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: ACRO_BAL_ROLL/PITCH")); } return false; } #if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED // check range finder if optflow enabled if (optflow.enabled() && !sonar.pre_arm_check()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: check range finder")); } return false; } #endif #if FRAME_CONFIG == HELI_FRAME // check helicopter parameters if (!motors.parameter_check()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check Heli Parameters")); } return false; } #endif // HELI_FRAME } // check throttle is above failsafe throttle // this is near the bottom to allow other failures to be displayed before checking pilot throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Collective below Failsafe")); #else gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Throttle below Failsafe")); #endif } return false; } } // if we've gotten this far then pre arm checks have completed set_pre_arm_check(true); return true; }
/* * check if we have breached the geo-fence */ void Plane::geofence_check(bool altitude_check_only) { if (!geofence_enabled()) { // switch back to the chosen control mode if still in // GUIDED to the return point if (geofence_state != NULL && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) && control_mode == GUIDED && geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && guided_WP_loc.lat == geofence_state->guided_lat && guided_WP_loc.lng == geofence_state->guided_lng) { geofence_state->old_switch_position = 254; set_mode(get_previous_mode()); } return; } /* allocate the geo-fence state if need be */ if (geofence_state == NULL || !geofence_state->boundary_uptodate) { geofence_load(); if (!geofence_enabled()) { // may have been disabled by load return; } } bool outside = false; uint8_t breach_type = FENCE_BREACH_NONE; struct Location loc; // Never trigger a fence breach in the final stage of landing if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { return; } if (geofence_state->floor_enabled && geofence_check_minalt()) { outside = true; breach_type = FENCE_BREACH_MINALT; } else if (geofence_check_maxalt()) { outside = true; breach_type = FENCE_BREACH_MAXALT; } else if (!altitude_check_only && ahrs.get_position(loc)) { Vector2l location; location.x = loc.lat; location.y = loc.lng; outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1); if (outside) { breach_type = FENCE_BREACH_BOUNDARY; } } if (!outside) { if (geofence_state->fence_triggered && !altitude_check_only) { // we have moved back inside the fence geofence_state->fence_triggered = false; gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK")); #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 0); #endif gcs_send_message(MSG_FENCE_STATUS); } // we're inside, all is good with the world return; } // we are outside the fence if (geofence_state->fence_triggered && (control_mode == GUIDED || g.fence_action == FENCE_ACTION_REPORT)) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; } // we are outside, and have not previously triggered. geofence_state->fence_triggered = true; geofence_state->breach_count++; geofence_state->breach_time = millis(); geofence_state->breach_type = breach_type; #if FENCE_TRIGGERED_PIN > 0 hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT); hal.gpio->write(FENCE_TRIGGERED_PIN, 1); #endif gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered")); gcs_send_message(MSG_FENCE_STATUS); // see what action the user wants switch (g.fence_action) { case FENCE_ACTION_REPORT: break; case FENCE_ACTION_GUIDED: case FENCE_ACTION_GUIDED_THR_PASS: // make sure we don't auto trim the surfaces on this mode change int8_t saved_auto_trim = g.auto_trim; g.auto_trim.set(0); set_mode(GUIDED); g.auto_trim.set(saved_auto_trim); if (g.fence_ret_rally != 0) { //return to a rally point guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); } else { //return to fence return point, not a rally point if (g.fence_retalt > 0) { //fly to the return point using fence_retalt guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt; } else if (g.fence_minalt >= g.fence_maxalt) { // invalid min/max, use RTL_altitude guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; } else { // fly to the return point, with an altitude half way between // min and max guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2; } guided_WP_loc.options = 0; guided_WP_loc.lat = geofence_state->boundary[0].x; guided_WP_loc.lng = geofence_state->boundary[0].y; } geofence_state->guided_lat = guided_WP_loc.lat; geofence_state->guided_lng = guided_WP_loc.lng; geofence_state->old_switch_position = oldSwitchPosition; setup_terrain_target_alt(guided_WP_loc); set_guided_WP(); if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) { guided_throttle_passthru = true; } break; } }
/* main flight mode dependent update code */ void Plane::update_flight_mode(void) { enum FlightMode effective_mode = control_mode; if (control_mode == AUTO && g.auto_fbw_steer) { effective_mode = FLY_BY_WIRE_A; } if (effective_mode != AUTO) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } switch (effective_mode) { case AUTO: handle_auto_mode(); break; case RTL: case LOITER: case GUIDED: calc_nav_roll(); calc_nav_pitch(); calc_throttle(); break; case TRAINING: { training_manual_roll = false; training_manual_pitch = false; // if the roll is past the set roll limit, then // we set target roll to the limit if (ahrs.roll_sensor >= roll_limit_cd) { nav_roll_cd = roll_limit_cd; } else if (ahrs.roll_sensor <= -roll_limit_cd) { nav_roll_cd = -roll_limit_cd; } else { training_manual_roll = true; nav_roll_cd = 0; } // if the pitch is past the set pitch limits, then // we set target pitch to the limit if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { nav_pitch_cd = aparm.pitch_limit_max_cd; } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { nav_pitch_cd = pitch_limit_min_cd; } else { training_manual_pitch = true; nav_pitch_cd = 0; } if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } break; } case ACRO: { // handle locked/unlocked control if (acro_state.locked_roll) { nav_roll_cd = acro_state.locked_roll_err; } else { nav_roll_cd = ahrs.roll_sensor; } if (acro_state.locked_pitch) { nav_pitch_cd = acro_state.locked_pitch_cd; } else { nav_pitch_cd = ahrs.pitch_sensor; } break; } case AUTOTUNE: case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); } adjust_nav_pitch_throttle(); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } if (failsafe.ch3_failsafe && g.short_fs_action == 2) { // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; channel_throttle->servo_out = 0; } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700); if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { auto_state.fbwa_tdrag_takeoff_mode = true; gcs_send_text_P(SEVERITY_LOW, PSTR("FBWA tdrag mode\n")); } } } break; } case FLY_BY_WIRE_B: // Thanks to Yury MonZon for the altitude limit code! nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); update_fbwb_speed_height(); break; case CRUISE: /* in CRUISE mode we use the navigation code to control roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ if ((channel_roll->control_in != 0 || rudder_input != 0)) { cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; } if (!cruise_state.locked_heading) { nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); } else { calc_nav_roll(); } update_fbwb_speed_height(); break; case STABILIZE: nav_roll_cd = 0; nav_pitch_cd = 0; // throttle is passthrough break; case CIRCLE: // we have no GPS installed and have lost radio contact // or we just want to fly around in a gentle circle w/o GPS, // holding altitude at the altitude we set when we // switched into the mode nav_roll_cd = roll_limit_cd / 3; update_load_factor(); calc_nav_pitch(); calc_throttle(); break; case MANUAL: // servo_out is for Sim control only // --------------------------------- channel_roll->servo_out = channel_roll->pwm_to_angle(); channel_pitch->servo_out = channel_pitch->pwm_to_angle(); steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 case INITIALISING: // handled elsewhere break; } }
void Rover::init_barometer(void) { gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("Calibrating barometer")); barometer.calibrate(); gcs_send_text_P(MAV_SEVERITY_WARNING, PSTR("barometer calibration complete")); }
/* * Determine if we have crashed */ void Plane::crash_detection_update(void) { if (control_mode != AUTO) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; return; } uint32_t now_ms = hal.scheduler->millis(); bool auto_launch_detected; bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); if (!is_flying()) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0); if (been_auto_flying || // failed hand launch auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches // has launched but is no longer flying. That's a crash on takeoff. crashed = true; } break; case AP_SpdHgtControl::FLIGHT_NORMAL: if (been_auto_flying) { crashed = true; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: if (been_auto_flying) { crashed = true; } // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // so ground crashes most likely can not be triggered from here. However, // a crash into a tree, for example, would be. break; case AP_SpdHgtControl::FLIGHT_LAND_FINAL: // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll if (been_auto_flying && !crash_state.checkHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; // did we "crash" within 75m of the landing location? Probably just a hard landing crashed_near_land_waypoint = get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. crash_state.debounce_timer_ms = now_ms + 2500; } crash_state.checkHardLanding = true; break; } // switch } else { crash_state.checkHardLanding = false; } if (!crashed) { // reset timer crash_state.debounce_timer_ms = 0; } else if (crash_state.debounce_timer_ms == 0) { // start timer crash_state.debounce_timer_ms = now_ms; } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken")); } else { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken")); } } else { if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } auto_state.land_complete = true; if (crashed_near_land_waypoint) { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected")); } else { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected")); } } } }
bool AP_Arming::ins_checks(bool report) { if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) { const AP_InertialSensor &ins = ahrs.get_ins(); if (! ins.get_gyro_health_all()) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: gyros not healthy!")); } return false; } if (!skip_gyro_cal && ! ins.gyro_calibrated_ok_all()) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: gyros not calibrated!")); } return false; } if (! ins.get_accel_health_all()) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: accels not healthy!")); } return false; } if (!ahrs.healthy()) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: AHRS not healthy!")); } return false; } if (!ins.accel_calibrated_ok_all()) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: 3D accel cal needed")); } return false; } #if INS_MAX_INSTANCES > 1 // check all accelerometers point in roughly same direction if (ins.get_accel_count() > 1) { const Vector3f &prime_accel_vec = ins.get_accel(); for(uint8_t i=0; i<ins.get_accel_count(); i++) { // get next accel vector const Vector3f &accel_vec = ins.get_accel(i); Vector3f vec_diff = accel_vec - prime_accel_vec; // allow for up to 0.75 m/s/s difference. Has to pass // in last 10 seconds float threshold = 0.75f; if (i >= 2) { /* we allow for a higher threshold for IMU3 as it runs at a different temperature to IMU1/IMU2, and is not used for accel data in the EKF */ threshold *= 3; } if (vec_diff.length() <= threshold) { last_accel_pass_ms[i] = hal.scheduler->millis(); } if (hal.scheduler->millis() - last_accel_pass_ms[i] > 10000) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Accelerometers")); } return false; } } } // check all gyros are giving consistent readings if (ins.get_gyro_count() > 1) { const Vector3f &prime_gyro_vec = ins.get_gyro(); for(uint8_t i=0; i<ins.get_gyro_count(); i++) { // get next gyro vector const Vector3f &gyro_vec = ins.get_gyro(i); Vector3f vec_diff = gyro_vec - prime_gyro_vec; // allow for up to 5 degrees/s difference. Pass if its // been OK in last 10 seconds if (vec_diff.length() <= radians(5)) { last_gyro_pass_ms[i] = hal.scheduler->millis(); } if (hal.scheduler->millis() - last_gyro_pass_ms[i] > 10000) { if (report) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent gyros")); } return false; } } } #endif } return true; }