Browse Source

Copter: raise EKF failure even if USB is connected

This will let EKF go bad if your PixHawk is connected to your laptop.
This doesn't seem to be a problem for the other vehicles.

This also allows the EKF to go bad in-flight if you happen to have
connected (against AP's recommendations) your companion computer to your
flight controller via USB.  Since people do this, it is better to have
the checks than not.
master
Peter Barker 7 years ago committed by Andrew Tridgell
parent
commit
70d159cb38
  1. 2
      ArduCopter/ArduCopter.cpp
  2. 3
      ArduCopter/Copter.h
  3. 1
      ArduCopter/GCS_Mavlink.cpp
  4. 4
      ArduCopter/ekf_check.cpp
  5. 16
      ArduCopter/system.cpp

2
ArduCopter/ArduCopter.cpp

@ -439,8 +439,6 @@ void Copter::one_hz_loop() @@ -439,8 +439,6 @@ void Copter::one_hz_loop()
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
check_usb_mux();
// log terrain data
terrain_logging();

3
ArduCopter/Copter.h

@ -316,7 +316,7 @@ private: @@ -316,7 +316,7 @@ private:
uint8_t logging_started : 1; // 6 // true if dataflash logging has started
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection
uint8_t usb_connected_unused : 1; // 9 // UNUSED
uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
@ -917,7 +917,6 @@ private: @@ -917,7 +917,6 @@ private:
bool ekf_position_ok();
bool optflow_position_ok();
void update_auto_armed();
void check_usb_mux(void);
bool should_log(uint32_t mask);
void set_default_frame_class();
MAV_TYPE get_frame_mav_type();

1
ArduCopter/GCS_Mavlink.cpp

@ -1591,7 +1591,6 @@ void Copter::mavlink_delay_cb() @@ -1591,7 +1591,6 @@ void Copter::mavlink_delay_cb()
last_5s = tnow;
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
}
check_usb_mux();
DataFlash.EnableWrites(true);
}

4
ArduCopter/ekf_check.cpp

@ -34,8 +34,8 @@ void Copter::ekf_check() @@ -34,8 +34,8 @@ void Copter::ekf_check()
return;
}
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors->armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
// return immediately if motors are not armed, or ekf check is disabled
if (!motors->armed() || (g.fs_ekf_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;

16
ArduCopter/system.cpp

@ -89,11 +89,6 @@ void Copter::init_ardupilot() @@ -89,11 +89,6 @@ void Copter::init_ardupilot()
barometer.init();
// 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();
// setup telem slots with serial ports
gcs().setup_uarts(serial_manager);
@ -417,17 +412,6 @@ void Copter::update_auto_armed() @@ -417,17 +412,6 @@ void Copter::update_auto_armed()
}
}
void Copter::check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
if (usb_check == ap.usb_connected) {
return;
}
// the user has switched to/from the telemetry port
ap.usb_connected = usb_check;
}
/*
should we log a message type now?
*/

Loading…
Cancel
Save