mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-120 AHRS: Tightened up the communication protocol so it runs more quickly. Looks ok in gdb but GCS not crashing right now (rendering error) so I can't test it properly.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1366 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
9bb085b7fc
commit
5662d85c4e
@ -144,17 +144,15 @@ struct attitude_solution {
|
||||
|
||||
struct altitude_sensor {
|
||||
float altitude;
|
||||
float pressure;
|
||||
float temperature;
|
||||
float updated;
|
||||
};
|
||||
|
||||
struct gps_sensor {
|
||||
float latitude;
|
||||
float longitude;
|
||||
float altitude;
|
||||
float NED[3];
|
||||
float heading;
|
||||
float groundspeed;
|
||||
float status;
|
||||
float quality;
|
||||
float updated;
|
||||
};
|
||||
|
||||
static struct mag_sensor mag_data;
|
||||
@ -173,7 +171,6 @@ static struct attitude_solution attitude_data;
|
||||
void process_spi_request(void);
|
||||
void downsample_data(void);
|
||||
void calibrate_sensors(void);
|
||||
void initialize_location();
|
||||
void converge_insgps();
|
||||
|
||||
/**
|
||||
@ -213,8 +210,6 @@ uint8_t gps_updated = FALSE;
|
||||
double BaseECEF[3] = {0, 0, 0};
|
||||
//! Rotation matrix from LLA to Rne
|
||||
float Rne[3][3];
|
||||
//! Current location in NED coordinates relative to base
|
||||
float NED[3];
|
||||
//! Indicates the communications are requesting a calibration
|
||||
uint8_t calibration_pending = FALSE;
|
||||
|
||||
@ -340,22 +335,15 @@ int main()
|
||||
|
||||
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
|
||||
|
||||
if ( gps_updated )
|
||||
if ( gps_data.updated )
|
||||
{
|
||||
if(BaseECEF[0] == 0 && BaseECEF[1] == 0 && BaseECEF[2] == 0)
|
||||
initialize_location();
|
||||
|
||||
// Convert to NED frame. Probably this will be moved to OP mainboard
|
||||
double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
|
||||
LLA2Base(LLA, BaseECEF, Rne, NED);
|
||||
|
||||
// Compute velocity from Heading and groundspeed
|
||||
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
|
||||
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
|
||||
vel[2] = 0;
|
||||
FullCorrection(mag, NED, vel, altitude_data.altitude);
|
||||
|
||||
gps_updated = FALSE;
|
||||
|
||||
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
|
||||
gps_data.updated = FALSE;
|
||||
}
|
||||
else
|
||||
MagCorrection(mag);
|
||||
@ -397,18 +385,6 @@ int main()
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize a location as the home point and compute rotation matrix to
|
||||
* get to NED coordinates
|
||||
*/
|
||||
void initialize_location()
|
||||
{
|
||||
double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
|
||||
RneFromLLA(LLA, Rne);
|
||||
LLA2ECEF(LLA ,BaseECEF);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Downsample the analog data
|
||||
* @return none
|
||||
@ -681,14 +657,6 @@ void process_spi_request(void)
|
||||
dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_ALTITUDE:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ALTITUDE);
|
||||
altitude_data.altitude = user_rx_v1.payload.user.v.req.altitude.altitude;
|
||||
altitude_data.pressure = user_rx_v1.payload.user.v.req.altitude.pressure;
|
||||
altitude_data.temperature = user_rx_v1.payload.user.v.req.altitude.temperature;
|
||||
dump_spi_message(PIOS_COM_AUX, "V", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_NORTH:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_NORTH);
|
||||
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
|
||||
@ -748,18 +716,6 @@ void process_spi_request(void)
|
||||
dump_spi_message(PIOS_COM_AUX, "C", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_GPS:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_GPS);
|
||||
gps_updated = TRUE;
|
||||
gps_data.latitude = user_rx_v1.payload.user.v.req.gps.latitude;
|
||||
gps_data.longitude = user_rx_v1.payload.user.v.req.gps.longitude;
|
||||
gps_data.altitude = user_rx_v1.payload.user.v.req.gps.altitude;
|
||||
gps_data.heading = user_rx_v1.payload.user.v.req.gps.heading;
|
||||
gps_data.groundspeed = user_rx_v1.payload.user.v.req.gps.groundspeed;
|
||||
gps_data.status = user_rx_v1.payload.user.v.req.gps.status;
|
||||
dump_spi_message(PIOS_COM_AUX, "G", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDERAW);
|
||||
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x = mag_data.raw.axis[0];
|
||||
@ -788,16 +744,49 @@ void process_spi_request(void)
|
||||
dump_spi_message(PIOS_COM_AUX, "R", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_ATTITUDE:
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDE);
|
||||
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q1 = attitude_data.quaternion.q1;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q2 = attitude_data.quaternion.q2;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q3 = attitude_data.quaternion.q3;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q4 = attitude_data.quaternion.q4;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.euler.roll = attitude_data.euler.roll;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.euler.pitch = attitude_data.euler.pitch;
|
||||
user_tx_v1.payload.user.v.rsp.attitude.euler.yaw = attitude_data.euler.yaw;
|
||||
dump_spi_message(PIOS_COM_AUX, "A", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
|
||||
case OPAHRS_MSG_V1_REQ_UPDATE:
|
||||
// process incoming data
|
||||
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_UPDATE);
|
||||
if(user_rx_v1.payload.user.v.req.update.barometer.updated)
|
||||
{
|
||||
altitude_data.altitude = user_rx_v1.payload.user.v.req.update.barometer.altitude;
|
||||
altitude_data.updated = user_rx_v1.payload.user.v.req.update.barometer.updated;
|
||||
}
|
||||
if(user_rx_v1.payload.user.v.req.update.gps.updated)
|
||||
{
|
||||
gps_data.updated = TRUE;
|
||||
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
|
||||
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
|
||||
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
|
||||
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
|
||||
gps_data.groundspeed = user_rx_v1.payload.user.v.req.update.gps.groundspeed;
|
||||
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
|
||||
}
|
||||
|
||||
// send out attitude/position estimate
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q1 = attitude_data.quaternion.q1;
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2;
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3;
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.roll = attitude_data.euler.roll;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.pitch = attitude_data.euler.pitch;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.yaw = attitude_data.euler.yaw;
|
||||
|
||||
// TODO: separate this from INSGPS
|
||||
user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0];
|
||||
user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1];
|
||||
user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2];
|
||||
user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0];
|
||||
user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1];
|
||||
user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];
|
||||
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2;
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3;
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.roll = attitude_data.euler.roll;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.pitch = attitude_data.euler.pitch;
|
||||
user_tx_v1.payload.user.v.rsp.update.euler.yaw = attitude_data.euler.yaw;
|
||||
dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
|
||||
lfsm_user_set_tx_v1 (&user_tx_v1);
|
||||
break;
|
||||
default:
|
||||
|
@ -76,14 +76,14 @@ static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void ahrscommsTask(void* parameters);
|
||||
static void load_baro_altitude(struct opahrs_msg_v1_req_altitude * altitude);
|
||||
static void load_baro_altitude(struct opahrs_msg_v1_req_update * update);
|
||||
static void load_gps_position(struct opahrs_msg_v1_req_update * update);
|
||||
static void load_magnetic_north(struct opahrs_msg_v1_req_north * north);
|
||||
static void load_position_actual(struct opahrs_msg_v1_req_gps * gps);
|
||||
static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration);
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
|
||||
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
|
||||
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
|
||||
static void update_calibration(struct opahrs_msg_v1_rsp_calibration * calibration);
|
||||
static void process_update(struct opahrs_msg_v1_rsp_update * update); // main information parser
|
||||
|
||||
static bool BaroAltitudeIsUpdatedFlag = false;
|
||||
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
|
||||
@ -91,10 +91,10 @@ static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
|
||||
BaroAltitudeIsUpdatedFlag = true;
|
||||
}
|
||||
|
||||
static bool PositionActualIsUpdatedFlag = false;
|
||||
static void PositionActualUpdatedCb(UAVObjEvent * ev)
|
||||
static bool GPSPositionIsUpdatedFlag = false;
|
||||
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
PositionActualIsUpdatedFlag = true;
|
||||
GPSPositionIsUpdatedFlag = true;
|
||||
}
|
||||
|
||||
static bool HomeLocationIsUpdatedFlag = false;
|
||||
@ -116,7 +116,7 @@ static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
|
||||
int32_t AHRSCommsInitialize(void)
|
||||
{
|
||||
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
|
||||
PositionActualConnectCallback(PositionActualUpdatedCb);
|
||||
PositionActualConnectCallback(GPSPositionUpdatedCb);
|
||||
HomeLocationConnectCallback(HomeLocationUpdatedCb);
|
||||
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
|
||||
|
||||
@ -128,8 +128,8 @@ int32_t AHRSCommsInitialize(void)
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint16_t attitude_errors = 0, attituderaw_errors = 0, position_errors = 0,
|
||||
home_errors = 0, altitude_errors = 0, calibration_errors;
|
||||
static uint16_t update_errors = 0, attituderaw_errors = 0,
|
||||
home_errors = 0, calibration_errors;
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
@ -175,48 +175,49 @@ static void ahrscommsTask(void* parameters)
|
||||
/* Update settings with latest value */
|
||||
AttitudeSettingsGet(&settings);
|
||||
|
||||
if ((result = PIOS_OPAHRS_GetAttitude(&rsp)) == OPAHRS_RESULT_OK) {
|
||||
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
|
||||
} else {
|
||||
/* Comms error */
|
||||
attitude_errors++;
|
||||
break;
|
||||
// If settings indicate, grab the raw and filtered data instead of estimate
|
||||
if (settings.UpdateRaw)
|
||||
{
|
||||
if( (result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
|
||||
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
|
||||
} else {
|
||||
/* Comms error */
|
||||
attituderaw_errors++;
|
||||
break;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
|
||||
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
|
||||
} else {
|
||||
/* Comms error */
|
||||
attituderaw_errors++;
|
||||
break;
|
||||
}
|
||||
// Otherwise do standard technique
|
||||
struct opahrs_msg_v1 req;
|
||||
struct opahrs_msg_v1 rsp;
|
||||
|
||||
// Load barometer if updated
|
||||
if (BaroAltitudeIsUpdatedFlag)
|
||||
load_baro_altitude(&(req.payload.user.v.req.update));
|
||||
else
|
||||
req.payload.user.v.req.update.barometer.updated = 0;
|
||||
|
||||
if (BaroAltitudeIsUpdatedFlag) {
|
||||
struct opahrs_msg_v1 req;
|
||||
// Load GPS if updated
|
||||
if (GPSPositionIsUpdatedFlag)
|
||||
load_gps_position(&(req.payload.user.v.req.update));
|
||||
else
|
||||
req.payload.user.v.req.update.gps.updated = 0;
|
||||
|
||||
load_baro_altitude(&(req.payload.user.v.req.altitude));
|
||||
if ((result = PIOS_OPAHRS_SetBaroAltitude(&req)) == OPAHRS_RESULT_OK) {
|
||||
// Transfer packet and process returned attitude
|
||||
if ((result = PIOS_OPAHRS_SetGetUpdate(&req,&rsp)) == OPAHRS_RESULT_OK) {
|
||||
if (req.payload.user.v.req.update.barometer.updated)
|
||||
BaroAltitudeIsUpdatedFlag = false;
|
||||
} else {
|
||||
/* Comms error */
|
||||
altitude_errors++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (PositionActualIsUpdatedFlag) {
|
||||
struct opahrs_msg_v1 req;
|
||||
|
||||
load_position_actual(&(req.payload.user.v.req.gps));
|
||||
if ((result = PIOS_OPAHRS_SetPositionActual(&req)) == OPAHRS_RESULT_OK) {
|
||||
PositionActualIsUpdatedFlag = false;
|
||||
} else {
|
||||
/* Comms error */
|
||||
position_errors++;
|
||||
break;
|
||||
}
|
||||
if (req.payload.user.v.req.update.gps.updated)
|
||||
GPSPositionIsUpdatedFlag = false;
|
||||
process_update(&(rsp.payload.user.v.rsp.update));
|
||||
} else {
|
||||
/* Comms error */
|
||||
update_errors++;
|
||||
break;
|
||||
}
|
||||
|
||||
// Update home coordinate if it hasn't been updated
|
||||
AhrsStatusGet(&data);
|
||||
if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
|
||||
struct opahrs_msg_v1 req;
|
||||
@ -235,6 +236,7 @@ static void ahrscommsTask(void* parameters)
|
||||
}
|
||||
}
|
||||
|
||||
// Update calibration if necessary
|
||||
AhrsStatusGet(&data);
|
||||
if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE))
|
||||
{
|
||||
@ -340,44 +342,52 @@ static void load_magnetic_north(struct opahrs_msg_v1_req_north * mag_north)
|
||||
|
||||
}
|
||||
|
||||
static void load_baro_altitude(struct opahrs_msg_v1_req_altitude * altitude)
|
||||
static void load_baro_altitude(struct opahrs_msg_v1_req_update * update)
|
||||
{
|
||||
BaroAltitudeData data;
|
||||
|
||||
BaroAltitudeGet(&data);
|
||||
|
||||
altitude->altitude = data.Altitude;
|
||||
altitude->pressure = data.Pressure;
|
||||
altitude->temperature = data.Temperature;
|
||||
update->barometer.altitude = data.Altitude;
|
||||
update->barometer.updated = 1;
|
||||
}
|
||||
|
||||
static void load_position_actual(struct opahrs_msg_v1_req_gps * gps)
|
||||
static void load_gps_position(struct opahrs_msg_v1_req_update * update)
|
||||
{
|
||||
PositionActualData data;
|
||||
PositionActualGet(&data);
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
gps->latitude = data.Latitude;
|
||||
gps->longitude = data.Longitude;
|
||||
gps->altitude = data.GeoidSeparation;
|
||||
gps->heading = data.Heading;
|
||||
gps->groundspeed = data.Groundspeed;
|
||||
gps->status = data.Status;
|
||||
|
||||
update->gps.updated = 1;
|
||||
update->gps.NED[0] = 0;
|
||||
update->gps.NED[1] = 0;
|
||||
update->gps.NED[2] = 0;
|
||||
update->gps.groundspeed = 0;
|
||||
update->gps.heading = 0;
|
||||
update->gps.quality = 0;
|
||||
}
|
||||
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
|
||||
static void process_update(struct opahrs_msg_v1_rsp_update * update)
|
||||
{
|
||||
AttitudeActualData data;
|
||||
|
||||
data.q1 = attitude->quaternion.q1;
|
||||
data.q2 = attitude->quaternion.q2;
|
||||
data.q3 = attitude->quaternion.q3;
|
||||
data.q4 = attitude->quaternion.q4;
|
||||
data.q1 = update->quaternion.q1;
|
||||
data.q2 = update->quaternion.q2;
|
||||
data.q3 = update->quaternion.q3;
|
||||
data.q4 = update->quaternion.q4;
|
||||
|
||||
data.Roll = attitude->euler.roll;
|
||||
data.Pitch = attitude->euler.pitch;
|
||||
data.Yaw = attitude->euler.yaw;
|
||||
data.Roll = update->euler.roll;
|
||||
data.Pitch = update->euler.pitch;
|
||||
data.Yaw = update->euler.yaw;
|
||||
|
||||
AttitudeActualSet(&data);
|
||||
|
||||
/*
|
||||
* update->NED[]
|
||||
* update->Vel[]
|
||||
*/
|
||||
}
|
||||
|
||||
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw)
|
||||
@ -421,11 +431,9 @@ static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
|
||||
data.SerialNumber[i] = serial->serial_bcd[i];
|
||||
}
|
||||
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDE] = attitude_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_UPDATE] = update_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_ATTITUDERAW] = attituderaw_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_POSITIONACTUAL] = position_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_ALTITUDE] = altitude_errors;
|
||||
data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors;
|
||||
|
||||
AhrsStatusSet(&data);
|
||||
|
@ -73,6 +73,8 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AttitudeSettingsData));
|
||||
data.Algorithm = 0;
|
||||
data.UpdateRaw = 0;
|
||||
data.UpdatePeriod = 500;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
@ -33,7 +33,7 @@
|
||||
#define AHRSSTATUS_H
|
||||
|
||||
// Object constants
|
||||
#define AHRSSTATUS_OBJID 1556283776U
|
||||
#define AHRSSTATUS_OBJID 1048419880U
|
||||
#define AHRSSTATUS_NAME "AhrsStatus"
|
||||
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
|
||||
#define AHRSSTATUS_ISSINGLEINST 1
|
||||
@ -58,7 +58,7 @@
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t SerialNumber[25];
|
||||
uint8_t CommErrors[6];
|
||||
uint8_t CommErrors[4];
|
||||
uint8_t HomeSet;
|
||||
uint8_t CalibrationSet;
|
||||
|
||||
@ -70,9 +70,9 @@ typedef struct {
|
||||
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
|
||||
// Field CommErrors information
|
||||
/* Array element names for field CommErrors */
|
||||
typedef enum { AHRSSTATUS_COMMERRORS_ATTITUDE=0, AHRSSTATUS_COMMERRORS_ATTITUDERAW=1, AHRSSTATUS_COMMERRORS_POSITIONACTUAL=2, AHRSSTATUS_COMMERRORS_HOMELOCATION=3, AHRSSTATUS_COMMERRORS_ALTITUDE=4, AHRSSTATUS_COMMERRORS_CALIBRATION=5 } AhrsStatusCommErrorsElem;
|
||||
typedef enum { AHRSSTATUS_COMMERRORS_UPDATE=0, AHRSSTATUS_COMMERRORS_ATTITUDERAW=1, AHRSSTATUS_COMMERRORS_HOMELOCATION=2, AHRSSTATUS_COMMERRORS_CALIBRATION=3 } AhrsStatusCommErrorsElem;
|
||||
/* Number of elements for field CommErrors */
|
||||
#define AHRSSTATUS_COMMERRORS_NUMELEM 6
|
||||
#define AHRSSTATUS_COMMERRORS_NUMELEM 4
|
||||
// Field HomeSet information
|
||||
/* Enumeration options for field HomeSet */
|
||||
typedef enum { AHRSSTATUS_HOMESET_FALSE=0, AHRSSTATUS_HOMESET_TRUE=1 } AhrsStatusHomeSetOptions;
|
||||
|
@ -33,7 +33,7 @@
|
||||
#define ATTITUDESETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define ATTITUDESETTINGS_OBJID 3446368842U
|
||||
#define ATTITUDESETTINGS_OBJID 2356162226U
|
||||
#define ATTITUDESETTINGS_NAME "AttitudeSettings"
|
||||
#define ATTITUDESETTINGS_METANAME "AttitudeSettingsMeta"
|
||||
#define ATTITUDESETTINGS_ISSINGLEINST 1
|
||||
@ -57,11 +57,19 @@
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Algorithm;
|
||||
uint8_t UpdateRaw;
|
||||
int32_t UpdatePeriod;
|
||||
|
||||
} __attribute__((packed)) AttitudeSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field Algorithm information
|
||||
/* Enumeration options for field Algorithm */
|
||||
typedef enum { ATTITUDESETTINGS_ALGORITHM_INSGPS=0 } AttitudeSettingsAlgorithmOptions;
|
||||
// Field UpdateRaw information
|
||||
/* Enumeration options for field UpdateRaw */
|
||||
typedef enum { ATTITUDESETTINGS_UPDATERAW_FALSE=0, ATTITUDESETTINGS_UPDATERAW_TRUE=1 } AttitudeSettingsUpdateRawOptions;
|
||||
// Field UpdatePeriod information
|
||||
|
||||
|
||||
|
@ -103,7 +103,7 @@ static enum opahrs_result opahrs_msg_v1_recv_rsp (enum opahrs_msg_v1_tag tag, st
|
||||
|
||||
opahrs_msg_v1_init_link_tx(&link_tx, OPAHRS_MSG_LINK_TAG_NOP);
|
||||
|
||||
for (uint8_t retries = 0; retries < 20; retries++) {
|
||||
for (uint8_t retries = 0; retries < 100; retries++) {
|
||||
if (opahrs_msg_txrx((const uint8_t *)&link_tx, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
|
||||
return OPAHRS_RESULT_FAILED;
|
||||
}
|
||||
@ -119,7 +119,7 @@ static enum opahrs_result opahrs_msg_v1_recv_rsp (enum opahrs_msg_v1_tag tag, st
|
||||
switch (rsp->payload.link.state) {
|
||||
case OPAHRS_MSG_LINK_STATE_BUSY:
|
||||
/* Wait for a small delay and retry */
|
||||
vTaskDelay(20 / portTICK_RATE_MS);
|
||||
vTaskDelay(40 / portTICK_RATE_MS);
|
||||
continue;
|
||||
case OPAHRS_MSG_LINK_STATE_INACTIVE:
|
||||
case OPAHRS_MSG_LINK_STATE_READY:
|
||||
@ -245,28 +245,6 @@ enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp)
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_ATTITUDERAW, rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 req;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!rsp) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up an attitude solution request */
|
||||
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_ATTITUDE);
|
||||
|
||||
/* Send the message until it is received */
|
||||
rc = opahrs_msg_v1_send_req (&req);
|
||||
if (rc != OPAHRS_RESULT_OK) {
|
||||
/* Failed to send the request, bail out */
|
||||
return rc;
|
||||
}
|
||||
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_ATTITUDE, rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req)
|
||||
{
|
||||
struct opahrs_msg_v1 rsp;
|
||||
@ -289,9 +267,8 @@ enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req)
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_NORTH, &rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetGetUpdate(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp)
|
||||
{
|
||||
struct opahrs_msg_v1 rsp;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!req) {
|
||||
@ -299,7 +276,7 @@ enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
|
||||
}
|
||||
|
||||
/* Make up an attituderaw request */
|
||||
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_GPS);
|
||||
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_UPDATE);
|
||||
|
||||
/* Send the message until it is received */
|
||||
rc = opahrs_msg_v1_send_req (req);
|
||||
@ -308,29 +285,7 @@ enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
|
||||
return rc;
|
||||
}
|
||||
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_GPS, &rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_SetBaroAltitude(struct opahrs_msg_v1 *req)
|
||||
{
|
||||
struct opahrs_msg_v1 rsp;
|
||||
enum opahrs_result rc;
|
||||
|
||||
if (!req) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Make up an attituderaw request */
|
||||
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_ALTITUDE);
|
||||
|
||||
/* Send the message until it is received */
|
||||
rc = opahrs_msg_v1_send_req (req);
|
||||
if (rc != OPAHRS_RESULT_OK) {
|
||||
/* Failed to send the request, bail out */
|
||||
return rc;
|
||||
}
|
||||
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_ALTITUDE, &rsp);
|
||||
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_UPDATE, rsp);
|
||||
}
|
||||
|
||||
enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp)
|
||||
|
@ -41,11 +41,9 @@ enum opahrs_result {
|
||||
extern void PIOS_OPAHRS_Init(void);
|
||||
extern enum opahrs_result PIOS_OPAHRS_Sync(struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_GetSerial(struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetGetUpdate(struct opahrs_msg_v1 *rsp, struct opahrs_msg_v1 *req);
|
||||
extern enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetBaroAltitude(struct opahrs_msg_v1 *req);
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req);
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req);
|
||||
extern enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp);
|
||||
extern enum opahrs_result PIOS_OPAHRS_resync(void);
|
||||
|
||||
|
@ -184,31 +184,27 @@ struct opahrs_msg_v1_req_reset {
|
||||
struct opahrs_msg_v1_req_serial {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_altitude {
|
||||
float altitude;
|
||||
float pressure;
|
||||
float temperature;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_north {
|
||||
float Be[3];
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_gps {
|
||||
float latitude;
|
||||
float longitude;
|
||||
float altitude;
|
||||
float heading;
|
||||
float groundspeed;
|
||||
uint8_t status;
|
||||
struct opahrs_msg_v1_req_update {
|
||||
struct {
|
||||
uint8_t updated;
|
||||
float NED[3];
|
||||
float groundspeed;
|
||||
float heading;
|
||||
float quality;
|
||||
} gps;
|
||||
struct {
|
||||
uint8_t updated;
|
||||
float altitude;
|
||||
} barometer;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_attituderaw {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_attitude {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_req_calibration {
|
||||
uint8_t measure_var;
|
||||
uint16_t accel_bias[3];
|
||||
@ -226,11 +222,9 @@ union opahrs_msg_v1_req {
|
||||
struct opahrs_msg_v1_req_sync sync;
|
||||
struct opahrs_msg_v1_req_reset reset;
|
||||
struct opahrs_msg_v1_req_serial serial;
|
||||
struct opahrs_msg_v1_req_altitude altitude;
|
||||
struct opahrs_msg_v1_req_update update;
|
||||
struct opahrs_msg_v1_req_north north;
|
||||
struct opahrs_msg_v1_req_gps gps;
|
||||
struct opahrs_msg_v1_req_attituderaw attituderaw;
|
||||
struct opahrs_msg_v1_req_attitude attitude;
|
||||
struct opahrs_msg_v1_req_calibration calibration;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
@ -247,15 +241,9 @@ struct opahrs_msg_v1_rsp_serial {
|
||||
uint8_t serial_bcd[25];
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_altitude {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_north {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_gps {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_attituderaw {
|
||||
struct {
|
||||
int16_t x;
|
||||
@ -286,7 +274,7 @@ struct opahrs_msg_v1_rsp_attituderaw {
|
||||
} accels_filtered;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_attitude {
|
||||
struct opahrs_msg_v1_rsp_update {
|
||||
struct {
|
||||
float q1;
|
||||
float q2;
|
||||
@ -298,6 +286,8 @@ struct opahrs_msg_v1_rsp_attitude {
|
||||
float pitch;
|
||||
float yaw;
|
||||
} euler;
|
||||
float NED[3];
|
||||
float Vel[3];
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_calibration {
|
||||
@ -311,11 +301,9 @@ struct opahrs_msg_v1_rsp_calibration {
|
||||
union opahrs_msg_v1_rsp {
|
||||
struct opahrs_msg_v1_rsp_sync sync;
|
||||
struct opahrs_msg_v1_rsp_serial serial;
|
||||
struct opahrs_msg_v1_rsp_altitude altitude;
|
||||
struct opahrs_msg_v1_rsp_north north;
|
||||
struct opahrs_msg_v1_rsp_gps gps;
|
||||
struct opahrs_msg_v1_rsp_attituderaw attituderaw;
|
||||
struct opahrs_msg_v1_rsp_attitude attitude;
|
||||
struct opahrs_msg_v1_rsp_update update;
|
||||
struct opahrs_msg_v1_rsp_calibration calibration;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
@ -324,20 +312,16 @@ enum opahrs_msg_v1_tag {
|
||||
OPAHRS_MSG_V1_REQ_SYNC,
|
||||
OPAHRS_MSG_V1_REQ_RESET,
|
||||
OPAHRS_MSG_V1_REQ_SERIAL,
|
||||
OPAHRS_MSG_V1_REQ_ALTITUDE,
|
||||
OPAHRS_MSG_V1_REQ_NORTH,
|
||||
OPAHRS_MSG_V1_REQ_GPS,
|
||||
OPAHRS_MSG_V1_REQ_UPDATE,
|
||||
OPAHRS_MSG_V1_REQ_ATTITUDERAW,
|
||||
OPAHRS_MSG_V1_REQ_ATTITUDE,
|
||||
OPAHRS_MSG_V1_REQ_CALIBRATION,
|
||||
|
||||
OPAHRS_MSG_V1_RSP_SYNC,
|
||||
OPAHRS_MSG_V1_RSP_SERIAL,
|
||||
OPAHRS_MSG_V1_RSP_ALTITUDE,
|
||||
OPAHRS_MSG_V1_RSP_NORTH,
|
||||
OPAHRS_MSG_V1_RSP_GPS,
|
||||
OPAHRS_MSG_V1_RSP_UPDATE,
|
||||
OPAHRS_MSG_V1_RSP_ATTITUDERAW,
|
||||
OPAHRS_MSG_V1_RSP_ATTITUDE,
|
||||
OPAHRS_MSG_V1_RSP_CALIBRATION,
|
||||
};
|
||||
|
||||
|
@ -20,6 +20,8 @@
|
||||
651CF9F1120B700D00EEFD70 /* pios_usb_hid_prop.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_usb_hid_prop.h; sourceTree = "<group>"; };
|
||||
651CF9F2120B700D00EEFD70 /* pios_usb_hid_pwr.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_usb_hid_pwr.h; sourceTree = "<group>"; };
|
||||
651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = "<group>"; };
|
||||
65209A1812208B0600453371 /* baroaltitude.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = baroaltitude.xml; sourceTree = "<group>"; };
|
||||
65209A1912208B0600453371 /* gpsposition.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpsposition.xml; sourceTree = "<group>"; };
|
||||
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
|
||||
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
|
||||
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
|
||||
@ -2546,7 +2548,6 @@
|
||||
65B367E5121C2620003EAD18 /* actuatordesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatordesired.xml; sourceTree = "<group>"; };
|
||||
65B367E6121C2620003EAD18 /* actuatorsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = actuatorsettings.xml; sourceTree = "<group>"; };
|
||||
65B367E7121C2620003EAD18 /* ahrsstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrsstatus.xml; sourceTree = "<group>"; };
|
||||
65B367E8121C2620003EAD18 /* altitudeactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = altitudeactual.xml; sourceTree = "<group>"; };
|
||||
65B367E9121C2620003EAD18 /* attitudeactual.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudeactual.xml; sourceTree = "<group>"; };
|
||||
65B367EA121C2620003EAD18 /* attitudedesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudedesired.xml; sourceTree = "<group>"; };
|
||||
65B367EB121C2620003EAD18 /* attituderaw.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attituderaw.xml; sourceTree = "<group>"; };
|
||||
@ -6668,14 +6669,14 @@
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
655268BC121FBD2900410C6E /* ahrscalibration.xml */,
|
||||
65B367E7121C2620003EAD18 /* ahrsstatus.xml */,
|
||||
65B367E4121C2620003EAD18 /* actuatorcommand.xml */,
|
||||
65B367E5121C2620003EAD18 /* actuatordesired.xml */,
|
||||
65B367E6121C2620003EAD18 /* actuatorsettings.xml */,
|
||||
65B367E7121C2620003EAD18 /* ahrsstatus.xml */,
|
||||
65B367E8121C2620003EAD18 /* altitudeactual.xml */,
|
||||
65B367E9121C2620003EAD18 /* attitudeactual.xml */,
|
||||
65B367EA121C2620003EAD18 /* attitudedesired.xml */,
|
||||
65B367EB121C2620003EAD18 /* attituderaw.xml */,
|
||||
65209A1812208B0600453371 /* baroaltitude.xml */,
|
||||
65B367EC121C2620003EAD18 /* attitudesettings.xml */,
|
||||
65B367ED121C2620003EAD18 /* exampleobject1.xml */,
|
||||
65B367EE121C2620003EAD18 /* exampleobject2.xml */,
|
||||
@ -6684,6 +6685,7 @@
|
||||
65B367F1121C2620003EAD18 /* flightsituationactual.xml */,
|
||||
65B367F2121C2620003EAD18 /* flighttelemetrystats.xml */,
|
||||
65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */,
|
||||
65209A1912208B0600453371 /* gpsposition.xml */,
|
||||
657CEEAD121DB6C8007A1FBE /* homelocation.xml */,
|
||||
65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */,
|
||||
65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */,
|
||||
|
@ -70,11 +70,9 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
|
||||
SerialNumberElemNames.append("24");
|
||||
fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) );
|
||||
QStringList CommErrorsElemNames;
|
||||
CommErrorsElemNames.append("Attitude");
|
||||
CommErrorsElemNames.append("Update");
|
||||
CommErrorsElemNames.append("AttitudeRaw");
|
||||
CommErrorsElemNames.append("PositionActual");
|
||||
CommErrorsElemNames.append("HomeLocation");
|
||||
CommErrorsElemNames.append("Altitude");
|
||||
CommErrorsElemNames.append("Calibration");
|
||||
fields.append( new UAVObjectField(QString("CommErrors"), QString("count"), UAVObjectField::UINT8, CommErrorsElemNames, QStringList()) );
|
||||
QStringList HomeSetElemNames;
|
||||
|
@ -44,7 +44,7 @@ public:
|
||||
// Field structure
|
||||
typedef struct {
|
||||
quint8 SerialNumber[25];
|
||||
quint8 CommErrors[6];
|
||||
quint8 CommErrors[4];
|
||||
quint8 HomeSet;
|
||||
quint8 CalibrationSet;
|
||||
|
||||
@ -56,9 +56,9 @@ public:
|
||||
static const quint32 SERIALNUMBER_NUMELEM = 25;
|
||||
// Field CommErrors information
|
||||
/* Array element names for field CommErrors */
|
||||
typedef enum { COMMERRORS_ATTITUDE=0, COMMERRORS_ATTITUDERAW=1, COMMERRORS_POSITIONACTUAL=2, COMMERRORS_HOMELOCATION=3, COMMERRORS_ALTITUDE=4, COMMERRORS_CALIBRATION=5 } CommErrorsElem;
|
||||
typedef enum { COMMERRORS_UPDATE=0, COMMERRORS_ATTITUDERAW=1, COMMERRORS_HOMELOCATION=2, COMMERRORS_CALIBRATION=3 } CommErrorsElem;
|
||||
/* Number of elements for field CommErrors */
|
||||
static const quint32 COMMERRORS_NUMELEM = 6;
|
||||
static const quint32 COMMERRORS_NUMELEM = 4;
|
||||
// Field HomeSet information
|
||||
/* Enumeration options for field HomeSet */
|
||||
typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions;
|
||||
@ -68,7 +68,7 @@ public:
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 1556283776U;
|
||||
static const quint32 OBJID = 1048419880U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 0;
|
||||
|
@ -74,13 +74,11 @@ _fields = [ \
|
||||
uavobject.UAVObjectField(
|
||||
'CommErrors',
|
||||
'B',
|
||||
6,
|
||||
4,
|
||||
[
|
||||
'Attitude',
|
||||
'Update',
|
||||
'AttitudeRaw',
|
||||
'PositionActual',
|
||||
'HomeLocation',
|
||||
'Altitude',
|
||||
'Calibration',
|
||||
],
|
||||
{
|
||||
@ -115,7 +113,7 @@ _fields = [ \
|
||||
|
||||
class AhrsStatus(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 1556283776
|
||||
OBJID = 1048419880
|
||||
NAME = "AhrsStatus"
|
||||
METANAME = "AhrsStatusMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
@ -42,6 +42,17 @@ AttitudeSettings::AttitudeSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTI
|
||||
{
|
||||
// Create fields
|
||||
QList<UAVObjectField*> fields;
|
||||
QStringList AlgorithmElemNames;
|
||||
AlgorithmElemNames.append("0");
|
||||
QStringList AlgorithmEnumOptions;
|
||||
AlgorithmEnumOptions.append("INSGPS");
|
||||
fields.append( new UAVObjectField(QString("Algorithm"), QString(""), UAVObjectField::ENUM, AlgorithmElemNames, AlgorithmEnumOptions) );
|
||||
QStringList UpdateRawElemNames;
|
||||
UpdateRawElemNames.append("0");
|
||||
QStringList UpdateRawEnumOptions;
|
||||
UpdateRawEnumOptions.append("FALSE");
|
||||
UpdateRawEnumOptions.append("TRUE");
|
||||
fields.append( new UAVObjectField(QString("UpdateRaw"), QString("raw"), UAVObjectField::ENUM, UpdateRawElemNames, UpdateRawEnumOptions) );
|
||||
QStringList UpdatePeriodElemNames;
|
||||
UpdatePeriodElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::INT32, UpdatePeriodElemNames, QStringList()) );
|
||||
@ -78,6 +89,8 @@ UAVObject::Metadata AttitudeSettings::getDefaultMetadata()
|
||||
*/
|
||||
void AttitudeSettings::setDefaultFieldValues()
|
||||
{
|
||||
data.Algorithm = 0;
|
||||
data.UpdateRaw = 0;
|
||||
data.UpdatePeriod = 500;
|
||||
|
||||
}
|
||||
|
@ -43,16 +43,24 @@ class UAVOBJECTS_EXPORT AttitudeSettings: public UAVDataObject
|
||||
public:
|
||||
// Field structure
|
||||
typedef struct {
|
||||
quint8 Algorithm;
|
||||
quint8 UpdateRaw;
|
||||
qint32 UpdatePeriod;
|
||||
|
||||
} __attribute__((packed)) DataFields;
|
||||
|
||||
// Field information
|
||||
// Field Algorithm information
|
||||
/* Enumeration options for field Algorithm */
|
||||
typedef enum { ALGORITHM_INSGPS=0 } AlgorithmOptions;
|
||||
// Field UpdateRaw information
|
||||
/* Enumeration options for field UpdateRaw */
|
||||
typedef enum { UPDATERAW_FALSE=0, UPDATERAW_TRUE=1 } UpdateRawOptions;
|
||||
// Field UpdatePeriod information
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 3446368842U;
|
||||
static const quint32 OBJID = 2356162226U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 1;
|
||||
|
@ -37,6 +37,29 @@ from collections import namedtuple
|
||||
|
||||
# This is a list of instances of the data fields contained in this object
|
||||
_fields = [ \
|
||||
uavobject.UAVObjectField(
|
||||
'Algorithm',
|
||||
'b',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
'0' : 'INSGPS',
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'UpdateRaw',
|
||||
'b',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
'0' : 'FALSE',
|
||||
'1' : 'TRUE',
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'UpdatePeriod',
|
||||
'i',
|
||||
@ -52,7 +75,7 @@ _fields = [ \
|
||||
|
||||
class AttitudeSettings(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 3446368842
|
||||
OBJID = 2356162226
|
||||
NAME = "AttitudeSettings"
|
||||
METANAME = "AttitudeSettingsMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
@ -1,7 +1,7 @@
|
||||
<xml>
|
||||
<object name="AhrsStatus" singleinstance="true" settings="false">
|
||||
<field name="SerialNumber" units="n/a" type="uint8" elements="25"/>
|
||||
<field name="CommErrors" units="count" type="uint8" elementnames="Attitude,AttitudeRaw,PositionActual,HomeLocation,Altitude,Calibration"/>
|
||||
<field name="CommErrors" units="count" type="uint8" elementnames="Update,AttitudeRaw,HomeLocation,Calibration"/>
|
||||
<field name="HomeSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
|
||||
<field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
|
@ -1,5 +1,7 @@
|
||||
<xml>
|
||||
<object name="AttitudeSettings" singleinstance="true" settings="true">
|
||||
<field name="Algorithm" units="" type="enum" elements="1" options="INSGPS" defaultvalue="INSGPS"/>
|
||||
<field name="UpdateRaw" units="raw" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
|
||||
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="500"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user