Browse Source

backup commit ,test branch

master
z 5 years ago
parent
commit
ab39f834aa
  1. 4
      ArduCopter/AP_Arming.cpp
  2. 16
      ArduCopter/UserCode.cpp
  3. 8
      git_submodule.sh
  4. 2
      libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp
  5. 8
      libraries/RC_Channel/RC_Channel.cpp
  6. 4
      libraries/RC_Channel/RC_Channel.h
  7. 2
      modules/mavlink

4
ArduCopter/AP_Arming.cpp

@ -30,6 +30,8 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
if (copter.motors->armed()) { if (copter.motors->armed()) {
return true; return true;
} }
if(AP_Arming::get_enabled_checks() == 0)
return true;
// check if motor interlock and Emergency Stop aux switches are used // check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed. // at the same time. This cannot be allowed.
@ -368,7 +370,7 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
// warn about hdop separately - to prevent user confusion with no gps lock // warn about hdop separately - to prevent user confusion with no gps lock
if (copter.gps.get_hdop() > copter.g.gps_hdop_good) { if (copter.gps.get_hdop() > copter.g.gps_hdop_good) {
check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP"); check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP %d (need %d)",copter.gps.get_hdop(),copter.g.gps_hdop_good);
AP_Notify::flags.pre_arm_gps_check = false; AP_Notify::flags.pre_arm_gps_check = false;
return false; return false;
} }

16
ArduCopter/UserCode.cpp

@ -82,8 +82,13 @@ void Copter::userhook_SuperSlowLoop()
{ {
// put your 1Hz code here // put your 1Hz code here
static bool before_fly = true; static bool before_fly = true;
if(motors->armed()) if(motors->armed()){
before_fly = false; before_fly = false;
relay.on(3);
}else{
relay.off(3);
}
if(before_fly){ if(before_fly){
uint8_t cnt,cacl_volt_pst; uint8_t cnt,cacl_volt_pst;
float delt_volt; float delt_volt;
@ -96,6 +101,15 @@ void Copter::userhook_SuperSlowLoop()
cacl_volt_pst = 100 - cnt; cacl_volt_pst = 100 - cnt;
battery.reset_remaining(0, cacl_volt_pst); battery.reset_remaining(0, cacl_volt_pst);
} }
// // check AHRS and GPS are within 10m of each other
// const Location gps_loc = gps.location();
// Location ahrs_loc;
// if (AP::ahrs().get_position(ahrs_loc)) {
// const float distance = gps_loc.get_distance(ahrs_loc);
// gcs().send_text(MAV_SEVERITY_INFO, "GPS and AHRS differ by %4.1fm", (double)distance);
// }
} }
#endif #endif

8
git_submodule.sh

@ -0,0 +1,8 @@
date -R
starttime=`date +'%Y-%m-%d %H:%M:%S'`
proxychains git submodule update --init --recursive
endtime=`date +'%Y-%m-%d %H:%M:%S'`
date -R
start_seconds=$(date --date="$starttime" +%s);
end_seconds=$(date --date="$endtime" +%s);
echo "本次运行时间: "$((end_seconds-start_seconds))"s"

2
libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp

@ -438,7 +438,7 @@ void AP_UAVCAN_Server::verify_nodes(AP_UAVCAN *ap_uavcan)
/* Only report if the node was verified, otherwise ignore /* Only report if the node was verified, otherwise ignore
as this could be just Bootloader to Application transition. */ as this could be just Bootloader to Application transition. */
if (isNodeIDVerified(curr_verifying_node)) { if (isNodeIDVerified(curr_verifying_node)) {
gcs().send_text(MAV_SEVERITY_ERROR, "UC Node %d Down!", curr_verifying_node); // gcs().send_text(MAV_SEVERITY_ERROR, "UC Node %d Down!", curr_verifying_node);
// remove verification flag for this node // remove verification flag for this node
verified_mask.clear(curr_verifying_node); verified_mask.clear(curr_verifying_node);
} }

8
libraries/RC_Channel/RC_Channel.cpp

@ -137,7 +137,13 @@ RC_Channel::update(void)
radio_in = override_value; radio_in = override_value;
} else if (!rc().ignore_receiver()) { } else if (!rc().ignore_receiver()) {
radio_in = hal.rcin->read(ch_in); radio_in = hal.rcin->read(ch_in);
} else { } else if(rc().ignore_ctrl_channel()){
if(ch_in < 7)
return false;
else
radio_in = hal.rcin->read(ch_in);
}
else {
return false; return false;
} }

4
libraries/RC_Channel/RC_Channel.h

@ -339,6 +339,9 @@ public:
bool ignore_receiver() const { bool ignore_receiver() const {
return _options & uint32_t(Option::IGNORE_RECEIVER); return _options & uint32_t(Option::IGNORE_RECEIVER);
} }
bool ignore_ctrl_channel() const {
return _options & uint32_t(Option::IGNORE_CTRLCHAN);
}
float override_timeout_ms() const { float override_timeout_ms() const {
return _override_timeout.get() * 1e3f; return _override_timeout.get() * 1e3f;
@ -350,6 +353,7 @@ protected:
IGNORE_RECEIVER = (1 << 0), // RC receiver modules IGNORE_RECEIVER = (1 << 0), // RC receiver modules
IGNORE_OVERRIDES = (1 << 1), // MAVLink overrides IGNORE_OVERRIDES = (1 << 1), // MAVLink overrides
IGNORE_FAILSAFE = (1 << 2), // ignore RC failsafe bits IGNORE_FAILSAFE = (1 << 2), // ignore RC failsafe bits
IGNORE_CTRLCHAN = (1 << 3), // ignore RC failsafe bits
}; };
void new_override_received() { void new_override_received() {

2
modules/mavlink

@ -1 +1 @@
Subproject commit ca507c39ff56b88ce78a072a691e4b0c2a95f527 Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019
Loading…
Cancel
Save