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

AP_ExternalAHRS: added frontend/backend split 

allow for multiple backends
This commit is contained in:
Andrew Tridgell 2021-03-24 17:24:01 +11:00
parent b3ed4f4b12
commit bb28a58bce
6 changed files with 734 additions and 478 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

488
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -16,90 +16,14 @@
suppport for serial connected AHRS systems
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "AP_ExternalAHRS.h"
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Common/NMEA.h>
#include <stdio.h>
#include "AP_ExternalAHRS_VectorNav.h"
#if HAL_EXTERNAL_AHRS_ENABLED
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal;
HAL_Semaphore AP_ExternalAHRS::sem;
/*
send requested config to the VN
*/
void AP_ExternalAHRS::send_config(void) const
{
nmea_printf(uart, "$VNWRG,75,3,%u,35,0003,0F2C,0147,0613", unsigned(400/rate.get()));
nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,2018");
}
/*
header for pre-configured 50Hz data
assumes the following config for VN-300:
$VNWRG,75,3,8,35,0003,0F2C,0147,0613*2642
*/
static const uint8_t vn_pkt1_header[] { 0x35, 0x03, 0x00, 0x2c, 0x0f, 0x47, 0x01, 0x13, 0x06 };
#define VN_PKT1_LENGTH 194 // includes header
struct PACKED VN_packet1 {
uint64_t timeStartup;
uint64_t timeGPS;
float uncompAccel[3];
float uncompAngRate[3];
float pressure;
float mag[3];
float accel[3];
float gyro[3];
uint16_t sensSat;
uint16_t AHRSStatus;
float ypr[3];
float quaternion[4];
float linAccBody[3];
float yprU[3];
uint16_t INSStatus;
double positionLLA[3];
float velNED[3];
float posU;
float velU;
};
// check packet size for 4 groups
static_assert(sizeof(VN_packet1)+2+4*2+2 == VN_PKT1_LENGTH, "incorrect VN_packet1 length");
/*
header for pre-configured 5Hz data
assumes the following VN-300 config:
$VNWRG,76,3,80,4E,0002,0010,20B8,2018*A66B
*/
static const uint8_t vn_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x20 };
#define VN_PKT2_LENGTH 120 // includes header
struct PACKED VN_packet2 {
uint64_t timeGPS;
float temp;
uint8_t numGPS1Sats;
uint8_t GPS1Fix;
double GPS1posLLA[3];
float GPS1velNED[3];
float GPS1DOP[7];
uint8_t numGPS2Sats;
uint8_t GPS2Fix;
float GPS2DOP[7];
};
// check packet size for 4 groups
static_assert(sizeof(VN_packet2)+2+4*2+2 == VN_PKT2_LENGTH, "incorrect VN_packet2 length");
AP_ExternalAHRS *AP_ExternalAHRS::_singleton;
@ -142,435 +66,139 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
void AP_ExternalAHRS::init(void)
{
if (devtype != DevType::VecNav) {
return;
}
auto &sm = AP::serialmanager();
uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
if (!uart) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS no UART");
return;
}
baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
bufsize = MAX(VN_PKT1_LENGTH, VN_PKT2_LENGTH);
pktbuf = new uint8_t[bufsize];
last_pkt1 = new VN_packet1;
last_pkt2 = new VN_packet2;
if (!pktbuf || !last_pkt1 || !last_pkt2) {
AP_HAL::panic("Failed to allocate ExternalAHRS");
if (rate.get() < 50) {
// min 50Hz
rate.set(50);
}
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
AP_HAL::panic("Failed to start ExternalAHRS update thread");
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised");
}
/*
check the UART for more data
returns true if the function should be called again straight away
*/
bool AP_ExternalAHRS::check_uart()
{
if (!port_opened) {
return false;
}
WITH_SEMAPHORE(sem);
uint32_t n = uart->available();
if (n == 0) {
return false;
}
if (pktoffset < bufsize) {
ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset)));
if (nread <= 0) {
return false;
}
pktoffset += nread;
}
bool match_header1, match_header2;
if (pktbuf[0] != 0xFA) {
goto reset;
}
match_header1 = (0 == memcmp(&pktbuf[1], vn_pkt1_header, MIN(sizeof(vn_pkt1_header), unsigned(pktoffset-1))));
match_header2 = (0 == memcmp(&pktbuf[1], vn_pkt2_header, MIN(sizeof(vn_pkt2_header), unsigned(pktoffset-1))));
if (!match_header1 && !match_header2) {
goto reset;
}
if (match_header1 && pktoffset >= VN_PKT1_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT1_LENGTH-1, 0);
if (crc == 0) {
// got pkt1
process_packet1(&pktbuf[sizeof(vn_pkt1_header)+1]);
memmove(&pktbuf[0], &pktbuf[VN_PKT1_LENGTH], pktoffset-VN_PKT1_LENGTH);
pktoffset -= VN_PKT1_LENGTH;
} else {
goto reset;
}
} else if (match_header2 && pktoffset >= VN_PKT2_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT2_LENGTH-1, 0);
if (crc == 0) {
// got pkt2
process_packet2(&pktbuf[sizeof(vn_pkt2_header)+1]);
memmove(&pktbuf[0], &pktbuf[VN_PKT2_LENGTH], pktoffset-VN_PKT2_LENGTH);
pktoffset -= VN_PKT2_LENGTH;
} else {
goto reset;
}
}
return true;
reset:
uint8_t *p = (uint8_t *)memchr(&pktbuf[1], (char)0xFA, pktoffset-1);
if (p) {
uint8_t newlen = pktoffset - (p - pktbuf);
memmove(&pktbuf[0], p, newlen);
pktoffset = newlen;
} else {
pktoffset = 0;
}
return true;
}
void AP_ExternalAHRS::update_thread()
{
if (!port_opened) {
// open port in the thread
port_opened = true;
uart->begin(baudrate, 1024, 512);
send_config();
}
while (true) {
if (!check_uart()) {
hal.scheduler->delay(1);
}
switch (DevType(devtype)) {
case DevType::VecNav:
backend = new AP_ExternalAHRS_VectorNav(this, state);
break;
default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype));
break;
}
}
/*
process packet type 1
*/
void AP_ExternalAHRS::process_packet1(const uint8_t *b)
{
const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)b;
const struct VN_packet2 &pkt2 = *last_pkt2;
last_pkt1_ms = AP_HAL::millis();
*last_pkt1 = pkt1;
{
baro_data_message_t baro;
baro.instance = 0;
baro.pressure_pa = pkt1.pressure*1e3;
baro.temperature = pkt2.temp;
AP::baro().handle_external(baro);
}
{
mag_data_message_t mag;
mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]};
mag.field *= 1000; // to mGauss
AP::compass().handle_external(mag);
}
{
ins_data_message_t ins;
ins.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]};
ins.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]};
ins.temperature = pkt2.temp;
AP::ins().handle_external(ins);
}
// @LoggerMessage: EAH1
// @Description: External AHRS data
// @Field: TimeUS: Time since system startup
// @Field: Roll: euler roll
// @Field: Pitch: euler pitch
// @Field: Yaw: euler yaw
// @Field: VN: velocity north
// @Field: VE: velocity east
// @Field: VD: velocity down
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
// @Field: UXY: uncertainty in XY position
// @Field: UV: uncertainty in velocity
// @Field: UR: uncertainty in roll
// @Field: UP: uncertainty in pitch
// @Field: UY: uncertainty in yaw
AP::logger().Write("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
"sdddnnnDUmmnddd", "F000000GG000000",
"QffffffLLffffff",
AP_HAL::micros64(),
pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
float(pkt1.positionLLA[2]),
pkt1.posU, pkt1.velU,
pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]);
}
/*
process packet type 2
*/
void AP_ExternalAHRS::process_packet2(const uint8_t *b)
{
const struct VN_packet2 &pkt2 = *(struct VN_packet2 *)b;
const struct VN_packet1 &pkt1 = *last_pkt1;
last_pkt2_ms = AP_HAL::millis();
*last_pkt2 = pkt2;
gps_data_message_t gps;
// get ToW in milliseconds
gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL);
gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL);
gps.fix_type = pkt2.GPS1Fix;
gps.satellites_in_view = pkt2.numGPS1Sats;
gps.horizontal_pos_accuracy = pkt1.posU;
gps.vertical_pos_accuracy = pkt1.posU;
gps.horizontal_vel_accuracy = pkt1.velU;
gps.hdop = pkt2.GPS1DOP[4];
gps.vdop = pkt2.GPS1DOP[3];
gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7;
gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7;
gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2;
gps.ned_vel_north = pkt2.GPS1velNED[0];
gps.ned_vel_east = pkt2.GPS1velNED[1];
gps.ned_vel_down = pkt2.GPS1velNED[2];
if (gps.fix_type >= 3 && !origin_set) {
origin.lat = gps.latitude;
origin.lng = gps.longitude;
origin.alt = gps.msl_altitude;
origin_set = true;
}
AP::gps().handle_external(gps);
}
// get serial port number for the uart
// get serial port number for the uart, or -1 if not applicable
int8_t AP_ExternalAHRS::get_port(void) const
{
if (!uart) {
if (!backend) {
return -1;
}
return port_num;
return backend->get_port();
};
// accessors for AP_AHRS
bool AP_ExternalAHRS::healthy(void) const
{
uint32_t now = AP_HAL::millis();
return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500);
return backend && backend->healthy();
}
bool AP_ExternalAHRS::initialised(void) const
{
return last_pkt1_ms != 0 && last_pkt2_ms != 0;
return backend && backend->initialised();
}
bool AP_ExternalAHRS::get_quaternion(Quaternion &quat)
{
if (!last_pkt1) {
return false;
if (state.have_quaternion) {
WITH_SEMAPHORE(state.sem);
quat = state.quat;
return true;
}
WITH_SEMAPHORE(sem);
const struct VN_packet1 &pkt1 = *last_pkt1;
quat(pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]);
return true;
return false;
}
bool AP_ExternalAHRS::get_origin(Location &loc)
{
WITH_SEMAPHORE(sem);
loc = origin;
return origin_set;
if (state.have_origin) {
WITH_SEMAPHORE(state.sem);
loc = state.origin;
return true;
}
return false;
}
bool AP_ExternalAHRS::get_location(Location &loc)
{
if (!last_pkt2) {
if (!state.have_location) {
return false;
}
const struct VN_packet2 &pkt2 = *last_pkt2;
WITH_SEMAPHORE(sem);
loc = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7),
int32_t(pkt2.GPS1posLLA[1] * 1.0e7),
int32_t(pkt2.GPS1posLLA[2] * 1.0e2),
Location::AltFrame::ABSOLUTE};
WITH_SEMAPHORE(state.sem);
loc = state.location;
return true;
}
Vector2f AP_ExternalAHRS::get_groundspeed_vector()
{
if (!last_pkt1) {
return Vector2f{};
}
const struct VN_packet1 &pkt1 = *last_pkt1;
WITH_SEMAPHORE(sem);
return Vector2f{pkt1.velNED[0], pkt1.velNED[1]};
WITH_SEMAPHORE(state.sem);
Vector2f vec{state.velocity.x, state.velocity.y};
return vec;
}
bool AP_ExternalAHRS::get_velocity_NED(Vector3f &vel)
{
if (!last_pkt1) {
if (!state.have_velocity) {
return false;
}
const struct VN_packet1 &pkt1 = *last_pkt1;
WITH_SEMAPHORE(sem);
vel.x = pkt1.velNED[0];
vel.y = pkt1.velNED[1];
vel.z = pkt1.velNED[2];
WITH_SEMAPHORE(state.sem);
vel = state.velocity;
return true;
}
bool AP_ExternalAHRS::get_speed_down(float &speedD)
{
if (!last_pkt1) {
if (!state.have_velocity) {
return false;
}
const struct VN_packet1 &pkt1 = *last_pkt1;
WITH_SEMAPHORE(sem);
speedD = pkt1.velNED[2];
WITH_SEMAPHORE(state.sem);
speedD = state.velocity.z;
return true;
}
bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy");
return false;
}
if (last_pkt2->GPS1Fix < 3) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock");
return false;
}
if (last_pkt2->GPS2Fix < 3) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock");
return false;
}
return true;
return backend && backend->pre_arm_check(failure_msg, failure_msg_len);
}
/*
get filter status. We don't know the meaning of the status bits yet,
so assume all OK if we have GPS lock
get filter status
*/
void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
{
memset(&status, 0, sizeof(status));
if (last_pkt1 && last_pkt2) {
status.flags.initalized = 1;
}
if (healthy() && last_pkt2) {
status.flags.attitude = 1;
status.flags.vert_vel = 1;
status.flags.vert_pos = 1;
const struct VN_packet2 &pkt2 = *last_pkt2;
if (pkt2.GPS1Fix >= 3) {
status.flags.horiz_vel = 1;
status.flags.horiz_pos_rel = 1;
status.flags.horiz_pos_abs = 1;
status.flags.pred_horiz_pos_rel = 1;
status.flags.pred_horiz_pos_abs = 1;
status.flags.using_gps = 1;
}
status = {};
if (backend) {
backend->get_filter_status(status);
}
}
Vector3f AP_ExternalAHRS::get_gyro(void)
{
if (!last_pkt1) {
return Vector3f{};
}
const struct VN_packet1 &pkt1 = *last_pkt1;
WITH_SEMAPHORE(sem);
return Vector3f(pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]);
WITH_SEMAPHORE(state.sem);
return state.gyro;
}
Vector3f AP_ExternalAHRS::get_accel(void)
{
if (!last_pkt1) {
return Vector3f{};
}
const struct VN_packet1 &pkt1 = *last_pkt1;
WITH_SEMAPHORE(sem);
return Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]};
WITH_SEMAPHORE(state.sem);
return state.accel;
}
// send an EKF_STATUS message to GCS
void AP_ExternalAHRS::send_status_report(mavlink_channel_t chan) const
{
if (!last_pkt1) {
return;
}
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
if (backend) {
backend->send_status_report(chan);
}
}
// send message
const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1;
const float vel_gate = 5;
const float pos_gate = 5;
const float hgt_gate = 5;
const float mag_var = 0;
mavlink_msg_ekf_status_report_send(chan, flags,
pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate,
mag_var, 0, 0);
void AP_ExternalAHRS::update(void)
{
if (backend) {
backend->update();
}
}
namespace AP {

92
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -31,9 +31,13 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_ExternalAHRS_backend;
class AP_ExternalAHRS {
public:
friend class AP_ExternalAHRS_backend;
AP_ExternalAHRS();
void init(void);
@ -49,7 +53,7 @@ public:
return _singleton;
}
// fixed IMU rate for now
// expected IMU rate in Hz
float get_IMU_rate(void) const {
return rate.get();
}
@ -57,6 +61,43 @@ public:
// get serial port number, -1 for not enabled
int8_t get_port(void) const;
struct state_t {
HAL_Semaphore sem;
Vector3f accel;
Vector3f gyro;
Quaternion quat;
Location location;
Vector3f velocity;
Location origin;
bool have_quaternion;
bool have_origin;
bool have_location;
bool have_velocity;
} state;
// accessors for AP_AHRS
bool healthy(void) const;
bool initialised(void) const;
bool get_quaternion(Quaternion &quat);
bool get_origin(Location &loc);
bool get_location(Location &loc);
Vector2f get_groundspeed_vector();
bool get_velocity_NED(Vector3f &vel);
bool get_speed_down(float &speedD);
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
void get_filter_status(nav_filter_status &status) const;
Vector3f get_gyro(void);
Vector3f get_accel(void);
void send_status_report(mavlink_channel_t chan) const;
// update backend
void update();
/*
structures passed to other subsystems
*/
typedef struct {
uint8_t instance;
float pressure_pa;
@ -90,59 +131,14 @@ public:
Vector3f gyro;
float temperature;
} ins_data_message_t;
// accessors for AP_AHRS
bool healthy(void) const;
bool initialised(void) const;
bool get_quaternion(Quaternion &quat);
bool get_origin(Location &loc);
bool get_location(Location &loc);
Vector2f get_groundspeed_vector();
bool get_velocity_NED(Vector3f &vel);
bool get_speed_down(float &speedD);
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
void get_filter_status(nav_filter_status &status) const;
Vector3f get_gyro(void);
Vector3f get_accel(void);
void send_status_report(mavlink_channel_t chan) const;
// check for new data
void update() {
check_uart();
}
private:
AP_HAL::UARTDriver *uart;
int8_t port_num;
bool port_opened;
uint32_t baudrate;
void update_thread();
bool check_uart();
void process_packet1(const uint8_t *b);
void process_packet2(const uint8_t *b);
void send_config(void) const;
AP_ExternalAHRS_backend *backend;
AP_Enum<DevType> devtype;
AP_Int16 rate;
static AP_ExternalAHRS *_singleton;
uint8_t *pktbuf;
uint16_t pktoffset;
uint16_t bufsize;
struct VN_packet1 *last_pkt1;
struct VN_packet2 *last_pkt2;
uint32_t last_pkt1_ms;
uint32_t last_pkt2_ms;
bool origin_set;
Location origin;
static HAL_Semaphore sem;
};
namespace AP {

472
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp Normal file
View File

@ -0,0 +1,472 @@
/*
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/>.
*/
/*
suppport for serial connected AHRS systems
*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "AP_ExternalAHRS_VectorNav.h"
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Common/NMEA.h>
#include <stdio.h>
#if HAL_EXTERNAL_AHRS_ENABLED
extern const AP_HAL::HAL &hal;
/*
send requested config to the VN
*/
void AP_ExternalAHRS_VectorNav::send_config(void) const
{
nmea_printf(uart, "$VNWRG,75,3,%u,35,0003,0F2C,0147,0613", unsigned(400/get_rate()));
nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,2018");
}
/*
header for pre-configured 50Hz data
assumes the following config for VN-300:
$VNWRG,75,3,8,35,0003,0F2C,0147,0613*2642
*/
static const uint8_t vn_pkt1_header[] { 0x35, 0x03, 0x00, 0x2c, 0x0f, 0x47, 0x01, 0x13, 0x06 };
#define VN_PKT1_LENGTH 194 // includes header
struct PACKED VN_packet1 {
uint64_t timeStartup;
uint64_t timeGPS;
float uncompAccel[3];
float uncompAngRate[3];
float pressure;
float mag[3];
float accel[3];
float gyro[3];
uint16_t sensSat;
uint16_t AHRSStatus;
float ypr[3];
float quaternion[4];
float linAccBody[3];
float yprU[3];
uint16_t INSStatus;
double positionLLA[3];
float velNED[3];
float posU;
float velU;
};
// check packet size for 4 groups
static_assert(sizeof(VN_packet1)+2+4*2+2 == VN_PKT1_LENGTH, "incorrect VN_packet1 length");
/*
header for pre-configured 5Hz data
assumes the following VN-300 config:
$VNWRG,76,3,80,4E,0002,0010,20B8,2018*A66B
*/
static const uint8_t vn_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x20 };
#define VN_PKT2_LENGTH 120 // includes header
struct PACKED VN_packet2 {
uint64_t timeGPS;
float temp;
uint8_t numGPS1Sats;
uint8_t GPS1Fix;
double GPS1posLLA[3];
float GPS1velNED[3];
float GPS1DOP[7];
uint8_t numGPS2Sats;
uint8_t GPS2Fix;
float GPS2DOP[7];
};
// check packet size for 4 groups
static_assert(sizeof(VN_packet2)+2+4*2+2 == VN_PKT2_LENGTH, "incorrect VN_packet2 length");
// constructor
AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend,
AP_ExternalAHRS::state_t &_state) :
AP_ExternalAHRS_backend(_frontend, _state)
{
auto &sm = AP::serialmanager();
uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
if (!uart) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS no UART");
return;
}
baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
bufsize = MAX(VN_PKT1_LENGTH, VN_PKT2_LENGTH);
pktbuf = new uint8_t[bufsize];
last_pkt1 = new VN_packet1;
last_pkt2 = new VN_packet2;
if (!pktbuf || !last_pkt1 || !last_pkt2) {
AP_HAL::panic("Failed to allocate ExternalAHRS");
}
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
AP_HAL::panic("Failed to start ExternalAHRS update thread");
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised");
}
/*
check the UART for more data
returns true if the function should be called again straight away
*/
bool AP_ExternalAHRS_VectorNav::check_uart()
{
if (!port_opened) {
return false;
}
WITH_SEMAPHORE(state.sem);
uint32_t n = uart->available();
if (n == 0) {
return false;
}
if (pktoffset < bufsize) {
ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset)));
if (nread <= 0) {
return false;
}
pktoffset += nread;
}
bool match_header1, match_header2;
if (pktbuf[0] != 0xFA) {
goto reset;
}
match_header1 = (0 == memcmp(&pktbuf[1], vn_pkt1_header, MIN(sizeof(vn_pkt1_header), unsigned(pktoffset-1))));
match_header2 = (0 == memcmp(&pktbuf[1], vn_pkt2_header, MIN(sizeof(vn_pkt2_header), unsigned(pktoffset-1))));
if (!match_header1 && !match_header2) {
goto reset;
}
if (match_header1 && pktoffset >= VN_PKT1_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT1_LENGTH-1, 0);
if (crc == 0) {
// got pkt1
process_packet1(&pktbuf[sizeof(vn_pkt1_header)+1]);
memmove(&pktbuf[0], &pktbuf[VN_PKT1_LENGTH], pktoffset-VN_PKT1_LENGTH);
pktoffset -= VN_PKT1_LENGTH;
} else {
goto reset;
}
} else if (match_header2 && pktoffset >= VN_PKT2_LENGTH) {
uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT2_LENGTH-1, 0);
if (crc == 0) {
// got pkt2
process_packet2(&pktbuf[sizeof(vn_pkt2_header)+1]);
memmove(&pktbuf[0], &pktbuf[VN_PKT2_LENGTH], pktoffset-VN_PKT2_LENGTH);
pktoffset -= VN_PKT2_LENGTH;
} else {
goto reset;
}
}
return true;
reset:
uint8_t *p = (uint8_t *)memchr(&pktbuf[1], (char)0xFA, pktoffset-1);
if (p) {
uint8_t newlen = pktoffset - (p - pktbuf);
memmove(&pktbuf[0], p, newlen);
pktoffset = newlen;
} else {
pktoffset = 0;
}
return true;
}
void AP_ExternalAHRS_VectorNav::update_thread()
{
if (!port_opened) {
// open port in the thread
port_opened = true;
uart->begin(baudrate, 1024, 512);
send_config();
}
while (true) {
if (!check_uart()) {
hal.scheduler->delay(1);
}
}
}
/*
process packet type 1
*/
void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b)
{
const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)b;
const struct VN_packet2 &pkt2 = *last_pkt2;
last_pkt1_ms = AP_HAL::millis();
*last_pkt1 = pkt1;
{
WITH_SEMAPHORE(state.sem);
state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]};
state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]};
state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]};
state.have_quaternion = true;
state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]};
state.have_velocity = true;
state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7),
int32_t(pkt1.positionLLA[1] * 1.0e7),
int32_t(pkt1.positionLLA[2] * 1.0e2),
Location::AltFrame::ABSOLUTE};
state.have_location = true;
}
{
AP_ExternalAHRS::baro_data_message_t baro;
baro.instance = 0;
baro.pressure_pa = pkt1.pressure*1e3;
baro.temperature = pkt2.temp;
AP::baro().handle_external(baro);
}
{
AP_ExternalAHRS::mag_data_message_t mag;
mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]};
mag.field *= 1000; // to mGauss
AP::compass().handle_external(mag);
}
{
AP_ExternalAHRS::ins_data_message_t ins;
ins.accel = state.accel;
ins.gyro = state.gyro;
ins.temperature = pkt2.temp;
AP::ins().handle_external(ins);
}
// @LoggerMessage: EAH1
// @Description: External AHRS data
// @Field: TimeUS: Time since system startup
// @Field: Roll: euler roll
// @Field: Pitch: euler pitch
// @Field: Yaw: euler yaw
// @Field: VN: velocity north
// @Field: VE: velocity east
// @Field: VD: velocity down
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
// @Field: UXY: uncertainty in XY position
// @Field: UV: uncertainty in velocity
// @Field: UR: uncertainty in roll
// @Field: UP: uncertainty in pitch
// @Field: UY: uncertainty in yaw
AP::logger().Write("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
"sdddnnnDUmmnddd", "F000000GG000000",
"QffffffLLffffff",
AP_HAL::micros64(),
pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
float(pkt1.positionLLA[2]),
pkt1.posU, pkt1.velU,
pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]);
}
/*
process packet type 2
*/
void AP_ExternalAHRS_VectorNav::process_packet2(const uint8_t *b)
{
const struct VN_packet2 &pkt2 = *(struct VN_packet2 *)b;
const struct VN_packet1 &pkt1 = *last_pkt1;
last_pkt2_ms = AP_HAL::millis();
*last_pkt2 = pkt2;
AP_ExternalAHRS::gps_data_message_t gps;
// get ToW in milliseconds
gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL);
gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL);
gps.fix_type = pkt2.GPS1Fix;
gps.satellites_in_view = pkt2.numGPS1Sats;
gps.horizontal_pos_accuracy = pkt1.posU;
gps.vertical_pos_accuracy = pkt1.posU;
gps.horizontal_vel_accuracy = pkt1.velU;
gps.hdop = pkt2.GPS1DOP[4];
gps.vdop = pkt2.GPS1DOP[3];
gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7;
gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7;
gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2;
gps.ned_vel_north = pkt2.GPS1velNED[0];
gps.ned_vel_east = pkt2.GPS1velNED[1];
gps.ned_vel_down = pkt2.GPS1velNED[2];
if (gps.fix_type >= 3 && !state.have_origin) {
WITH_SEMAPHORE(state.sem);
state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7),
int32_t(pkt2.GPS1posLLA[1] * 1.0e7),
int32_t(pkt2.GPS1posLLA[2] * 1.0e2),
Location::AltFrame::ABSOLUTE};
state.have_origin = true;
}
AP::gps().handle_external(gps);
}
// get serial port number for the uart
int8_t AP_ExternalAHRS_VectorNav::get_port(void) const
{
if (!uart) {
return -1;
}
return port_num;
};
// accessors for AP_AHRS
bool AP_ExternalAHRS_VectorNav::healthy(void) const
{
uint32_t now = AP_HAL::millis();
return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500);
}
bool AP_ExternalAHRS_VectorNav::initialised(void) const
{
return last_pkt1_ms != 0 && last_pkt2_ms != 0;
}
bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
if (!healthy()) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy");
return false;
}
if (last_pkt2->GPS1Fix < 3) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock");
return false;
}
if (last_pkt2->GPS2Fix < 3) {
hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock");
return false;
}
return true;
}
/*
get filter status. We don't know the meaning of the status bits yet,
so assume all OK if we have GPS lock
*/
void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const
{
memset(&status, 0, sizeof(status));
if (last_pkt1 && last_pkt2) {
status.flags.initalized = 1;
}
if (healthy() && last_pkt2) {
status.flags.attitude = 1;
status.flags.vert_vel = 1;
status.flags.vert_pos = 1;
const struct VN_packet2 &pkt2 = *last_pkt2;
if (pkt2.GPS1Fix >= 3) {
status.flags.horiz_vel = 1;
status.flags.horiz_pos_rel = 1;
status.flags.horiz_pos_abs = 1;
status.flags.pred_horiz_pos_rel = 1;
status.flags.pred_horiz_pos_abs = 1;
status.flags.using_gps = 1;
}
}
}
// send an EKF_STATUS message to GCS
void AP_ExternalAHRS_VectorNav::send_status_report(mavlink_channel_t chan) const
{
if (!last_pkt1) {
return;
}
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
// send message
const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1;
const float vel_gate = 5;
const float pos_gate = 5;
const float hgt_gate = 5;
const float mag_var = 0;
mavlink_msg_ekf_status_report_send(chan, flags,
pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate,
mag_var, 0, 0);
}
#endif // HAL_EXTERNAL_AHRS_ENABLED

73
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h Normal file
View File

@ -0,0 +1,73 @@
/*
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/>.
*/
/*
suppport for serial connected AHRS systems
*/
#pragma once
#include "AP_ExternalAHRS_backend.h"
#if HAL_EXTERNAL_AHRS_ENABLED
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
public:
AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
// get serial port number, -1 for not enabled
int8_t get_port(void) const override;
// accessors for AP_AHRS
bool healthy(void) const override;
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(mavlink_channel_t chan) const override;
// check for new data
void update() override {
check_uart();
}
private:
AP_HAL::UARTDriver *uart;
int8_t port_num;
bool port_opened;
uint32_t baudrate;
uint16_t rate;
void update_thread();
bool check_uart();
void process_packet1(const uint8_t *b);
void process_packet2(const uint8_t *b);
void send_config(void) const;
uint8_t *pktbuf;
uint16_t pktoffset;
uint16_t bufsize;
struct VN_packet1 *last_pkt1;
struct VN_packet2 *last_pkt2;
uint32_t last_pkt1_ms;
uint32_t last_pkt2_ms;
};
#endif // HAL_EXTERNAL_AHRS_ENABLED

36
libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp Normal file
View File

@ -0,0 +1,36 @@
/*
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/>.
*/
/*
parent class for ExternalAHRS backends
*/
#include "AP_ExternalAHRS_backend.h"
#if HAL_EXTERNAL_AHRS_ENABLED
AP_ExternalAHRS_backend::AP_ExternalAHRS_backend(AP_ExternalAHRS *_frontend,
AP_ExternalAHRS::state_t &_state) :
frontend(*_frontend),
state(_state)
{}
uint16_t AP_ExternalAHRS_backend::get_rate(void) const
{
return frontend.get_IMU_rate();
}
#endif // HAL_EXTERNAL_AHRS_ENABLED

51
libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h Normal file
View File

@ -0,0 +1,51 @@
/*
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/>.
*/
/*
parent class for ExternalAHRS backends
*/
#pragma once
#include "AP_ExternalAHRS.h"
#if HAL_EXTERNAL_AHRS_ENABLED
class AP_ExternalAHRS_backend {
public:
AP_ExternalAHRS_backend(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
// get serial port number, -1 for not enabled
virtual int8_t get_port(void) const { return -1; }
// accessors for AP_AHRS
virtual bool healthy(void) const = 0;
virtual bool initialised(void) const = 0;
virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0;
virtual void get_filter_status(nav_filter_status &status) const {}
virtual void send_status_report(mavlink_channel_t chan) const {}
// check for new data
virtual void update() = 0;
protected:
AP_ExternalAHRS::state_t &state;
uint16_t get_rate(void) const;
private:
AP_ExternalAHRS &frontend;
};
#endif // HAL_EXTERNAL_AHRS_ENABLED