mirror of https://github.com/ArduPilot/ardupilot
Copter: add AP_Rally_Copter
override default is_valid method so we can check if rally point is inside the fence, if it isn't we ignore it
This commit is contained in:
parent
ef28be9ce8
commit
25fefe77b7
|
@ -0,0 +1,32 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common/Location.h>
|
||||
|
||||
#include "Copter.h"
|
||||
|
||||
#include "AP_Rally.h"
|
||||
|
||||
bool AP_Rally_Copter::is_valid(const Location &rally_point) const
|
||||
{
|
||||
#if AC_FENCE == ENABLED
|
||||
Location_Class rally_loc(rally_point);
|
||||
if (!copter.fence.check_destination_within_fence(rally_loc)) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
|
@ -0,0 +1,32 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_Rally/AP_Rally.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
class AP_Rally_Copter : public AP_Rally
|
||||
{
|
||||
|
||||
public:
|
||||
// constructor
|
||||
AP_Rally_Copter(AP_AHRS &ahrs) : AP_Rally(ahrs) {};
|
||||
|
||||
private:
|
||||
bool is_valid(const Location &rally_point) const override;
|
||||
|
||||
};
|
|
@ -50,7 +50,6 @@
|
|||
#include <AP_NavEKF/AP_NavEKF.h>
|
||||
#include <AP_NavEKF2/AP_NavEKF2.h>
|
||||
#include <AP_Mission/AP_Mission.h> // Mission command library
|
||||
#include <AP_Rally/AP_Rally.h> // Rally point library
|
||||
#include <AC_PID/AC_PID.h> // PID library
|
||||
#include <AC_PID/AC_PI_2D.h> // PID library (2-axis)
|
||||
#include <AC_PID/AC_HELI_PID.h> // Heli specific Rate PID library
|
||||
|
@ -95,6 +94,7 @@
|
|||
#include "config.h"
|
||||
|
||||
#include "GCS_Mavlink.h"
|
||||
#include "AP_Rally.h" // Rally point library
|
||||
|
||||
// libraries which are dependent on #defines in defines.h and/or config.h
|
||||
#if SPRAYER == ENABLED
|
||||
|
@ -121,6 +121,7 @@
|
|||
class Copter : public AP_HAL::HAL::Callbacks {
|
||||
public:
|
||||
friend class GCS_MAVLINK_Copter;
|
||||
friend class AP_Rally_Copter;
|
||||
friend class Parameters;
|
||||
|
||||
Copter(void);
|
||||
|
@ -508,7 +509,7 @@ private:
|
|||
|
||||
// Rally library
|
||||
#if AC_RALLY == ENABLED
|
||||
AP_Rally rally;
|
||||
AP_Rally_Copter rally;
|
||||
#endif
|
||||
|
||||
// RSSI
|
||||
|
|
|
@ -831,8 +831,8 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
|
||||
#if AC_RALLY == ENABLED
|
||||
// @Group: RALLY_
|
||||
// @Path: ../libraries/AP_Rally/AP_Rally.cpp
|
||||
GOBJECT(rally, "RALLY_", AP_Rally),
|
||||
// @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp
|
||||
GOBJECT(rally, "RALLY_", AP_Rally_Copter),
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
|
Loading…
Reference in New Issue