Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: remove HIL support

This commit is contained in:
Peter Barker 2021-06-09 21:31:34 +10:00 committed by Andrew Tridgell
parent 304bc2bc13
commit a9621ed802
4 changed files with 10 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_Compass/AP_Compass.cpp
View File

@ -12,7 +12,6 @@
#include "AP_Compass_AK8963.h"
#include "AP_Compass_Backend.h"
#include "AP_Compass_BMM150.h"
#include "AP_Compass_HIL.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_IST8308.h"
#include "AP_Compass_IST8310.h"
@ -1194,13 +1193,6 @@ void Compass::_probe_external_i2c_compasses(void)
*/
void Compass::_detect_backends(void)
{
#ifndef HAL_BUILD_AP_PERIPH
if (_hil_mode) {
_add_backend(AP_Compass_HIL::detect());
return;
}
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) {
ADD_BACKEND(DRIVER_SERIAL, new AP_Compass_ExternalAHRS(serial_port));
@ -1235,8 +1227,6 @@ void Compass::_detect_backends(void)
#if defined(HAL_MAG_PROBE_LIST)
// driver probes defined by COMPASS lines in hwdef.dat
HAL_MAG_PROBE_LIST;
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
ADD_BACKEND(DRIVER_SITL, AP_Compass_HIL::detect());
#elif AP_FEATURE_BOARD_DETECT
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
@ -1866,68 +1856,20 @@ bool Compass::configured(char *failure_msg, uint8_t failure_msg_len)
return all_configured;
}
// Update raw magnetometer values from HIL data
//
void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
{
Matrix3f R;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
if (!is_equal(_hil.last_declination,get_declination())) {
_setup_earth_field();
_hil.last_declination = get_declination();
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
_hil.field[instance] = R.mul_transpose(_hil.Bearth);
// apply default board orientation for this compass type. This is
// a noop on most boards
_hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil.field[instance].rotate((enum Rotation)_state[StateIndex(0)].orientation.get());
if (!_state[StateIndex(0)].external) {
// and add in AHRS_ORIENTATION setting if not an external compass
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
_hil.field[instance] = *_custom_rotation * _hil.field[instance];
} else {
_hil.field[instance].rotate(_board_orientation);
}
}
_hil.healthy[instance] = true;
}
// Update raw magnetometer values from HIL mag vector
//
void Compass::setHIL(uint8_t instance, const Vector3f &mag, uint32_t update_usec)
{
_hil.field[instance] = mag;
_hil.healthy[instance] = true;
_state[StateIndex(instance)].last_update_usec = update_usec;
}
const Vector3f& Compass::getHIL(uint8_t instance) const
{
return _hil.field[instance];
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_hil.Bearth = {400, 0, 0};
_sitl.Bearth = {400, 0, 0};
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), get_declination());
_hil.Bearth = R * _hil.Bearth;
_sitl.Bearth = R * _sitl.Bearth;
}
#endif
/*
set the type of motor compensation to use

20
libraries/AP_Compass/AP_Compass.h
View File

@ -289,14 +289,9 @@ public:
bool configured(uint8_t i);
bool configured(char *failure_msg, uint8_t failure_msg_len);
// HIL methods
void setHIL(uint8_t instance, float roll, float pitch, float yaw);
void setHIL(uint8_t instance, const Vector3f &mag, uint32_t last_update_usec);
const Vector3f& getHIL(uint8_t instance) const;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
void _setup_earth_field();
// enable HIL mode
void set_hil_mode(void) { _hil_mode = true; }
#endif
// return last update time in microseconds
uint32_t last_update_usec(void) const { return last_update_usec(_first_usable); }
@ -310,10 +305,10 @@ public:
// HIL variables
struct {
Vector3f Bearth;
float last_declination;
bool healthy[COMPASS_MAX_INSTANCES];
Vector3f field[COMPASS_MAX_INSTANCES];
} _hil;
// float last_declination;
// bool healthy[COMPASS_MAX_INSTANCES];
// Vector3f field[COMPASS_MAX_INSTANCES];
} _sitl;
enum LearnType {
LEARN_NONE=0,
@ -590,9 +585,6 @@ private:
Compass_PerMotor _per_motor{*this};
#endif
// if we want HIL only
bool _hil_mode:1;
AP_Float _calibration_threshold;
// mask of driver types to not load. Bit positions match DEVTYPE_ in backend

76
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -1,76 +0,0 @@
/*
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/>.
*/
/*
* AP_Compass_HIL.cpp - HIL backend for AP_Compass
*
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_Compass_HIL.h"
extern const AP_HAL::HAL& hal;
// constructor
AP_Compass_HIL::AP_Compass_HIL()
{
memset(_compass_instance, 0, sizeof(_compass_instance));
_compass._setup_earth_field();
}
// detect the sensor
AP_Compass_Backend *AP_Compass_HIL::detect()
{
AP_Compass_HIL *sensor = new AP_Compass_HIL();
if (sensor == nullptr) {
return nullptr;
}
if (!sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool
AP_Compass_HIL::init(void)
{
// register compass instances
for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
uint32_t dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL);
if (!register_compass(dev_id, _compass_instance[i])) {
return false;
}
set_dev_id(_compass_instance[i], dev_id);
}
return true;
}
void AP_Compass_HIL::read()
{
for (uint8_t i=0; i < ARRAY_SIZE(_compass_instance); i++) {
if (_compass._hil.healthy[i]) {
uint8_t compass_instance = _compass_instance[i];
Vector3f field = _compass._hil.field[compass_instance];
rotate_field(field, compass_instance);
publish_raw_field(field, compass_instance);
correct_field(field, compass_instance);
uint32_t saved_last_update = _compass.last_update_usec(compass_instance);
publish_filtered_field(field, compass_instance);
set_last_update_usec(saved_last_update, compass_instance);
}
}
}

19
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -1,19 +0,0 @@
#pragma once
#include "AP_Compass.h"
#define HIL_NUM_COMPASSES 1
class AP_Compass_HIL : public AP_Compass_Backend
{
public:
AP_Compass_HIL();
void read(void) override;
bool init(void);
// detect the sensor
static AP_Compass_Backend *detect();
private:
uint8_t _compass_instance[HIL_NUM_COMPASSES];
};