/*
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/>.
*/
/*
support for serial connected InertialLabs INS system
*/
#pragma once
#include "AP_ExternalAHRS_config.h"
#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED
#include "AP_ExternalAHRS_backend.h"
class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
public:
AP_ExternalAHRS_InertialLabs(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(class GCS_MAVLINK &link) const override;
// check for new data
void update() override {
check_uart();
}
// Get model/type name
const char* get_name() const override {
return "ILabs";
}
enum class MessageType : uint8_t {
GPS_INS_TIME_MS = 0x01,
GPS_WEEK = 0x3C,
ACCEL_DATA_HR = 0x23,
GYRO_DATA_HR = 0x21,
BARO_DATA = 0x25,
MAG_DATA = 0x24,
ORIENTATION_ANGLES = 0x07,
VELOCITIES = 0x12,
POSITION = 0x10,
KF_VEL_COVARIANCE = 0x58,
KF_POS_COVARIANCE = 0x57,
UNIT_STATUS = 0x53,
GNSS_EXTENDED_INFO = 0x4A,
NUM_SATS = 0x3B,
GNSS_POSITION = 0x30,
GNSS_VEL_TRACK = 0x32,
GNSS_POS_TIMESTAMP = 0x3E,
GNSS_INFO_SHORT = 0x36,
GNSS_NEW_DATA = 0x41,
GNSS_JAM_STATUS = 0xC0,
DIFFERENTIAL_PRESSURE = 0x28,
TRUE_AIRSPEED = 0x86,
WIND_SPEED = 0x8A,
AIR_DATA_STATUS = 0x8D,
SUPPLY_VOLTAGE = 0x50,
TEMPERATURE = 0x52,
UNIT_STATUS2 = 0x5A,
};
/*
packets consist of:
ILabsHeader
list of MessageType
sequence of ILabsData
checksum
*/
struct PACKED ILabsHeader {
uint16_t magic; // 0x55AA
uint8_t msg_type; // always 1 for INS data
uint8_t msg_id; // always 0x95
uint16_t msg_len; // msg_len+2 is total packet length
};
struct PACKED vec3_16_t {
int16_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_32_t {
int32_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_u8_t {
uint8_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_u16_t {
uint16_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct gnss_extended_info_t {
uint8_t fix_type;
uint8_t spoofing_status;
};
struct gnss_info_short_t {
uint8_t info1;
uint8_t info2;
};
union PACKED ILabsData {
uint32_t gps_time_ms; // ms since start of GPS week
uint16_t gps_week;
vec3_32_t accel_data_hr; // g * 1e6
vec3_32_t gyro_data_hr; // deg/s * 1e5
struct PACKED {
uint16_t pressure_pa2; // Pascals/2
int32_t baro_alt; // meters*100
} baro_data;
vec3_16_t mag_data; // nT/10
struct PACKED {
int16_t yaw; // deg*100
int16_t pitch; // deg*100
int16_t roll; // deg*100
} orientation_angles; // 321 euler order?
vec3_32_t velocity; // m/s * 100
struct PACKED {
int32_t lat; // deg*1e7
int32_t lon; // deg*1e7
int32_t alt; // m*100, AMSL
} position;
vec3_u8_t kf_vel_covariance; // mm/s
vec3_u16_t kf_pos_covariance;
uint16_t unit_status; // set ILABS_UNIT_STATUS_*
gnss_extended_info_t gnss_extended_info;
uint8_t num_sats;
struct PACKED {
int32_t hor_speed; // m/s*100
uint16_t track_over_ground; // deg*100
int32_t ver_speed; // m/s*100
} gnss_vel_track;
uint32_t gnss_pos_timestamp; // ms
gnss_info_short_t gnss_info_short;
uint8_t gnss_new_data;
uint8_t gnss_jam_status;
int32_t differential_pressure; // mbar*1e4
int16_t true_airspeed; // m/s*100
vec3_16_t wind_speed; // m/s*100
uint16_t air_data_status;
uint16_t supply_voltage; // V*100
int16_t temperature; // degC*10
uint16_t unit_status2;
};
AP_ExternalAHRS::gps_data_message_t gps_data;
AP_ExternalAHRS::mag_data_message_t mag_data;
AP_ExternalAHRS::baro_data_message_t baro_data;
AP_ExternalAHRS::ins_data_message_t ins_data;
AP_ExternalAHRS::airspeed_data_message_t airspeed_data;
uint16_t buffer_ofs;
uint8_t buffer[256]; // max for normal message set is 167+8
private:
AP_HAL::UARTDriver *uart;
int8_t port_num;
uint32_t baudrate;
bool setup_complete;
void update_thread();
bool check_uart();
bool check_header(const ILabsHeader *h) const;
// re-sync on header bytes
void re_sync(void);
static const struct MessageLength {
MessageType mtype;
uint8_t length;
} message_lengths[];
struct {
Vector3f kf_vel_covariance;
Vector3f kf_pos_covariance;
uint32_t gnss_ins_time_ms;
uint16_t unit_status;
uint16_t unit_status2;
gnss_extended_info_t gnss_extended_info;
gnss_info_short_t gnss_info_short;
uint8_t gnss_new_data;
uint8_t gnss_jam_status;
float differential_pressure;
float true_airspeed;
Vector3f wind_speed;
uint16_t air_data_status;
float supply_voltage;
} state2;
uint32_t last_att_ms;
uint32_t last_vel_ms;
uint32_t last_pos_ms;
uint32_t last_gps_ms;
};
#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED