AP_Beacon: add and use AP_BEACON_ENABLED

This commit is contained in:
Peter Barker 2022-01-28 11:09:43 +11:00 committed by Peter Barker
parent 1e9d0f59c0
commit 694ad90ee7
13 changed files with 85 additions and 10 deletions

View File

@ -14,6 +14,9 @@
*/ */
#include "AP_Beacon.h" #include "AP_Beacon.h"
#if AP_BEACON_ENABLED
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#include "AP_Beacon_Pozyx.h" #include "AP_Beacon_Pozyx.h"
#include "AP_Beacon_Marvelmind.h" #include "AP_Beacon_Marvelmind.h"
@ -426,3 +429,5 @@ AP_Beacon *beacon()
} }
} }
#endif

View File

@ -14,6 +14,10 @@
*/ */
#pragma once #pragma once
#include "AP_Beacon_config.h"
#if AP_BEACON_ENABLED
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
@ -148,3 +152,5 @@ private:
namespace AP { namespace AP {
AP_Beacon *beacon(); AP_Beacon *beacon();
}; };
#endif // AP_BEACON_ENABLED

View File

@ -14,6 +14,9 @@
*/ */
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#if AP_BEACON_ENABLED
// debug // debug
#include <stdio.h> #include <stdio.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
@ -105,3 +108,5 @@ Vector3f AP_Beacon_Backend::correct_for_orient_yaw(const Vector3f &vector)
vec_rotated.z = vector.z; vec_rotated.z = vector.z;
return vec_rotated; return vec_rotated;
} }
#endif // AP_BEACON_ENABLED

View File

@ -14,10 +14,13 @@
*/ */
#pragma once #pragma once
#include "AP_Beacon.h"
#if AP_BEACON_ENABLED
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_Beacon.h"
class AP_Beacon_Backend class AP_Beacon_Backend
{ {
@ -62,3 +65,5 @@ protected:
AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *uart;
}; };
#endif // AP_BEACON_ENABLED

View File

@ -18,9 +18,12 @@
April 2017 April 2017
*/ */
#include "AP_Beacon_Marvelmind.h"
#if AP_BEACON_MARVELMIND_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include "AP_Beacon_Marvelmind.h"
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002
@ -386,3 +389,5 @@ void AP_Beacon_Marvelmind::order_stationary_beacons()
} while(swapped); } while(swapped);
} }
} }
#endif // AP_BEACON_MARVELMIND_ENABLED

View File

@ -22,6 +22,8 @@
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#if AP_BEACON_MARVELMIND_ENABLED
#define AP_BEACON_MARVELMIND_BUF_SIZE 255 #define AP_BEACON_MARVELMIND_BUF_SIZE 255
class AP_Beacon_Marvelmind : public AP_Beacon_Backend class AP_Beacon_Marvelmind : public AP_Beacon_Backend
@ -101,3 +103,4 @@ private:
bool beacon_position_initialized; bool beacon_position_initialized;
}; };
#endif // AP_BEACON_MARVELMIND_ENABLED

View File

@ -13,9 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_Beacon_Nooploop.h"
#if AP_BEACON_NOOPLOOP_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include "AP_Beacon_Nooploop.h"
#include <ctype.h> #include <ctype.h>
#include <stdio.h> #include <stdio.h>
@ -248,3 +251,5 @@ void AP_Beacon_Nooploop::parse_setting_frame0()
} }
_anchor_pos_avail = true; _anchor_pos_avail = true;
} }
#endif // AP_BEACON_NOOPLOOP_ENABLED

View File

@ -2,6 +2,8 @@
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#if AP_BEACON_NOOPLOOP_ENABLED
#define NOOPLOOP_MSG_BUF_MAX 256 #define NOOPLOOP_MSG_BUF_MAX 256
class AP_Beacon_Nooploop : public AP_Beacon_Backend class AP_Beacon_Nooploop : public AP_Beacon_Backend
@ -56,3 +58,5 @@ private:
bool _anchor_pos_avail; // flag indicates if we got anchor position or not bool _anchor_pos_avail; // flag indicates if we got anchor position or not
uint32_t _last_request_setting_ms; // last time we sent request_setting0 packet to tag uint32_t _last_request_setting_ms; // last time we sent request_setting0 packet to tag
}; };
#endif // AP_BEACON_NOOPLOOP_ENABLED

View File

@ -13,8 +13,11 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <AP_HAL/AP_HAL.h>
#include "AP_Beacon_Pozyx.h" #include "AP_Beacon_Pozyx.h"
#if AP_BEACON_POZYX_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <ctype.h> #include <ctype.h>
#include <stdio.h> #include <stdio.h>
@ -157,3 +160,5 @@ void AP_Beacon_Pozyx::parse_buffer()
last_update_ms = AP_HAL::millis(); last_update_ms = AP_HAL::millis();
} }
} }
#endif // AP_BEACON_POZYX_ENABLED

View File

@ -2,6 +2,8 @@
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#if AP_BEACON_POZYX_ENABLED
#define AP_BEACON_POZYX_MSG_LEN_MAX 20 // messages from uno/pozyx are no more than 20bytes #define AP_BEACON_POZYX_MSG_LEN_MAX 20 // messages from uno/pozyx are no more than 20bytes
#define AP_BEACON_POZYX_HEADER 0x01 // messages start with this character #define AP_BEACON_POZYX_HEADER 0x01 // messages start with this character
#define AP_BEACON_POZYX_MSGID_BEACON_CONFIG 0x02 // message contains anchor config information #define AP_BEACON_POZYX_MSGID_BEACON_CONFIG 0x02 // message contains anchor config information
@ -41,3 +43,5 @@ private:
uint8_t linebuf_len = 0; uint8_t linebuf_len = 0;
uint32_t last_update_ms = 0; uint32_t last_update_ms = 0;
}; };
#endif // AP_BEACON_POZYX_ENABLED

View File

@ -13,11 +13,12 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_Beacon_SITL.h"
#if AP_BEACON_SITL_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "AP_Beacon_SITL.h"
#include <stdio.h> #include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -110,4 +111,4 @@ void AP_Beacon_SITL::update(void)
last_update_ms = now; last_update_ms = now;
} }
#endif // CONFIG_HAL_BOARD #endif // AP_BEACON_SITL_ENABLED

View File

@ -2,7 +2,7 @@
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if AP_BEACON_SITL_ENABLED
#include <SITL/SITL.h> #include <SITL/SITL.h>
@ -25,4 +25,4 @@ private:
uint32_t last_update_ms; uint32_t last_update_ms;
}; };
#endif // CONFIG_HAL_BOARD #endif // AP_BEACON_SITL_ENABLED

View File

@ -0,0 +1,27 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_BEACON_ENABLED
#define AP_BEACON_ENABLED 1
#endif
#ifndef AP_BEACON_BACKEND_DEFAULT_ENABLED
#define AP_BEACON_BACKEND_DEFAULT_ENABLED AP_BEACON_ENABLED
#endif
#ifndef AP_BEACON_MARVELMIND_ENABLED
#define AP_BEACON_MARVELMIND_ENABLED AP_BEACON_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_BEACON_NOOPLOOP_ENABLED
#define AP_BEACON_NOOPLOOP_ENABLED AP_BEACON_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_BEACON_POZYX_ENABLED
#define AP_BEACON_POZYX_ENABLED AP_BEACON_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_BEACON_SITL_ENABLED
#define AP_BEACON_SITL_ENABLED (AP_BEACON_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif