Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pre-arm check that rally points are within fence

Browse Source
This commit is contained in:
Randy Mackay 2016-07-02 17:16:00 +09:00
parent 2647bed484
commit 877a144dea
2 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -913,6 +913,7 @@ private:
void pre_arm_rc_checks();
bool pre_arm_gps_checks(bool display_failure);
bool pre_arm_ekf_attitude_check();
bool pre_arm_rallypoint_check();
bool pre_arm_terrain_check(bool display_failure);
bool arm_checks(bool display_failure, bool arming_from_gcs);
void init_disarm_motors();

35
ArduCopter/arming_checks.cpp
View File

@ -174,6 +174,14 @@ bool Copter::pre_arm_checks(bool display_failure)
}
#endif
// check rally points
if (!pre_arm_rallypoint_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: rallypoints outside fence");
}
return false;
}
// check INS
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
@ -491,6 +499,25 @@ bool Copter::pre_arm_ekf_attitude_check()
return filt_status.flags.attitude;
}
// check rally points are within fences
bool Copter::pre_arm_rallypoint_check()
{
#if AC_RALLY == ENABLED && AC_FENCE == ENABLED
for (uint8_t i=0; i<rally.get_rally_total(); i++) {
RallyLocation rally_loc;
if (rally.get_rally_point_with_index(i, rally_loc)) {
Location_Class rally_point(rally.rally_location_to_location(rally_loc));
if (!fence.check_destination_within_fence(rally_point)) {
return false;
}
}
}
return true;
#else
return true;
#endif
}
// check we have required terrain data
bool Copter::pre_arm_terrain_check(bool display_failure)
{
@ -650,6 +677,14 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
}
#endif
// check rally points
if (!pre_arm_rallypoint_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: rallypoints outside fence");
}
return false;
}
// 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) {