rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1366 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-22 00:21:54 +00:00 committed by peabody124
parent 9bb085b7fc
commit 5662d85c4e
17 changed files with 227 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
flight/AHRS/ahrs.c
View File

@ -144,17 +144,15 @@ struct attitude_solution {
struct altitude_sensor { struct altitude_sensor {
float altitude; float altitude;
float pressure; float updated;
float temperature;
}; };
struct gps_sensor { struct gps_sensor {
float latitude; float NED[3];
float longitude;
float altitude;
float heading; float heading;
float groundspeed; float groundspeed;
float status; float quality;
float updated;
}; };
static struct mag_sensor mag_data; static struct mag_sensor mag_data;
@ -173,7 +171,6 @@ static struct attitude_solution attitude_data;
void process_spi_request(void); void process_spi_request(void);
void downsample_data(void); void downsample_data(void);
void calibrate_sensors(void); void calibrate_sensors(void);
void initialize_location();
void converge_insgps(); void converge_insgps();
/** /**
@ -213,8 +210,6 @@ uint8_t gps_updated = FALSE;
double BaseECEF[3] = {0, 0, 0}; double BaseECEF[3] = {0, 0, 0};
//! Rotation matrix from LLA to Rne //! Rotation matrix from LLA to Rne
float Rne[3][3]; float Rne[3][3];
//! Current location in NED coordinates relative to base
float NED[3];
//! Indicates the communications are requesting a calibration //! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE; uint8_t calibration_pending = FALSE;
@ -340,22 +335,15 @@ int main()
INSPrediction(gyro, accel, 1 / (float) EKF_RATE); 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 // Compute velocity from Heading and groundspeed
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180); 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[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
vel[2] = 0; vel[2] = 0;
FullCorrection(mag, NED, vel, altitude_data.altitude);
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
gps_updated = FALSE; gps_data.updated = FALSE;
} }
else else
MagCorrection(mag); MagCorrection(mag);
@ -397,18 +385,6 @@ int main()
return 0; 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 * @brief Downsample the analog data
* @return none * @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)); dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1); lfsm_user_set_tx_v1 (&user_tx_v1);
break; 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: case OPAHRS_MSG_V1_REQ_NORTH:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_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); 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)); dump_spi_message(PIOS_COM_AUX, "C", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1); lfsm_user_set_tx_v1 (&user_tx_v1);
break; 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: case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_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]; 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)); dump_spi_message(PIOS_COM_AUX, "R", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1); lfsm_user_set_tx_v1 (&user_tx_v1);
break; break;
case OPAHRS_MSG_V1_REQ_ATTITUDE: case OPAHRS_MSG_V1_REQ_UPDATE:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDE); // process incoming data
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q1 = attitude_data.quaternion.q1; opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_UPDATE);
user_tx_v1.payload.user.v.rsp.attitude.quaternion.q2 = attitude_data.quaternion.q2; if(user_rx_v1.payload.user.v.req.update.barometer.updated)
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; altitude_data.altitude = user_rx_v1.payload.user.v.req.update.barometer.altitude;
user_tx_v1.payload.user.v.rsp.attitude.euler.roll = attitude_data.euler.roll; altitude_data.updated = user_rx_v1.payload.user.v.req.update.barometer.updated;
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; if(user_rx_v1.payload.user.v.req.update.gps.updated)
dump_spi_message(PIOS_COM_AUX, "A", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); {
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); lfsm_user_set_tx_v1 (&user_tx_v1);
break; break;
default: default:

140
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -76,14 +76,14 @@ static xTaskHandle taskHandle;
// Private functions // Private functions
static void ahrscommsTask(void* parameters); 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_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 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_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial); 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 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 bool BaroAltitudeIsUpdatedFlag = false;
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev) static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
@ -91,10 +91,10 @@ static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
BaroAltitudeIsUpdatedFlag = true; BaroAltitudeIsUpdatedFlag = true;
} }
static bool PositionActualIsUpdatedFlag = false; static bool GPSPositionIsUpdatedFlag = false;
static void PositionActualUpdatedCb(UAVObjEvent * ev) static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{ {
PositionActualIsUpdatedFlag = true; GPSPositionIsUpdatedFlag = true;
} }
static bool HomeLocationIsUpdatedFlag = false; static bool HomeLocationIsUpdatedFlag = false;
@ -116,7 +116,7 @@ static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
int32_t AHRSCommsInitialize(void) int32_t AHRSCommsInitialize(void)
{ {
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb); BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
PositionActualConnectCallback(PositionActualUpdatedCb); PositionActualConnectCallback(GPSPositionUpdatedCb);
HomeLocationConnectCallback(HomeLocationUpdatedCb); HomeLocationConnectCallback(HomeLocationUpdatedCb);
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb); AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
@ -128,8 +128,8 @@ int32_t AHRSCommsInitialize(void)
return 0; return 0;
} }
static uint16_t attitude_errors = 0, attituderaw_errors = 0, position_errors = 0, static uint16_t update_errors = 0, attituderaw_errors = 0,
home_errors = 0, altitude_errors = 0, calibration_errors; home_errors = 0, calibration_errors;
/** /**
* Module thread, should not return. * Module thread, should not return.
@ -175,48 +175,49 @@ static void ahrscommsTask(void* parameters)
/* Update settings with latest value */ /* Update settings with latest value */
AttitudeSettingsGet(&settings); AttitudeSettingsGet(&settings);
if ((result = PIOS_OPAHRS_GetAttitude(&rsp)) == OPAHRS_RESULT_OK) { // If settings indicate, grab the raw and filtered data instead of estimate
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude)); if (settings.UpdateRaw)
} else { {
/* Comms error */ if( (result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
attitude_errors++; update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
break; } else {
/* Comms error */
attituderaw_errors++;
break;
}
continue;
} }
if ((result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) { // Otherwise do standard technique
update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw)); struct opahrs_msg_v1 req;
} else { struct opahrs_msg_v1 rsp;
/* Comms error */
attituderaw_errors++; // Load barometer if updated
break; if (BaroAltitudeIsUpdatedFlag)
} load_baro_altitude(&(req.payload.user.v.req.update));
else
req.payload.user.v.req.update.barometer.updated = 0;
if (BaroAltitudeIsUpdatedFlag) { // Load GPS if updated
struct opahrs_msg_v1 req; 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)); // Transfer packet and process returned attitude
if ((result = PIOS_OPAHRS_SetBaroAltitude(&req)) == OPAHRS_RESULT_OK) { if ((result = PIOS_OPAHRS_SetGetUpdate(&req,&rsp)) == OPAHRS_RESULT_OK) {
if (req.payload.user.v.req.update.barometer.updated)
BaroAltitudeIsUpdatedFlag = false; BaroAltitudeIsUpdatedFlag = false;
} else { if (req.payload.user.v.req.update.gps.updated)
/* Comms error */ GPSPositionIsUpdatedFlag = false;
altitude_errors++; process_update(&(rsp.payload.user.v.rsp.update));
break; } else {
} /* Comms error */
} update_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;
}
} }
// Update home coordinate if it hasn't been updated
AhrsStatusGet(&data); AhrsStatusGet(&data);
if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) { if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
struct opahrs_msg_v1 req; struct opahrs_msg_v1 req;
@ -235,6 +236,7 @@ static void ahrscommsTask(void* parameters)
} }
} }
// Update calibration if necessary
AhrsStatusGet(&data); AhrsStatusGet(&data);
if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE)) 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; BaroAltitudeData data;
BaroAltitudeGet(&data); BaroAltitudeGet(&data);
altitude->altitude = data.Altitude; update->barometer.altitude = data.Altitude;
altitude->pressure = data.Pressure; update->barometer.updated = 1;
altitude->temperature = data.Temperature;
} }
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; PositionActualData data;
PositionActualGet(&data); PositionActualGet(&data);
HomeLocationData home;
HomeLocationGet(&home);
gps->latitude = data.Latitude;
gps->longitude = data.Longitude; update->gps.updated = 1;
gps->altitude = data.GeoidSeparation; update->gps.NED[0] = 0;
gps->heading = data.Heading; update->gps.NED[1] = 0;
gps->groundspeed = data.Groundspeed; update->gps.NED[2] = 0;
gps->status = data.Status; 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; AttitudeActualData data;
data.q1 = attitude->quaternion.q1; data.q1 = update->quaternion.q1;
data.q2 = attitude->quaternion.q2; data.q2 = update->quaternion.q2;
data.q3 = attitude->quaternion.q3; data.q3 = update->quaternion.q3;
data.q4 = attitude->quaternion.q4; data.q4 = update->quaternion.q4;
data.Roll = attitude->euler.roll; data.Roll = update->euler.roll;
data.Pitch = attitude->euler.pitch; data.Pitch = update->euler.pitch;
data.Yaw = attitude->euler.yaw; data.Yaw = update->euler.yaw;
AttitudeActualSet(&data); AttitudeActualSet(&data);
/*
* update->NED[]
* update->Vel[]
*/
} }
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw) 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.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_ATTITUDERAW] = attituderaw_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_POSITIONACTUAL] = position_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors; data.CommErrors[AHRSSTATUS_COMMERRORS_HOMELOCATION] = home_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_ALTITUDE] = altitude_errors;
data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors; data.CommErrors[AHRSSTATUS_COMMERRORS_CALIBRATION] = calibration_errors;
AhrsStatusSet(&data); AhrsStatusSet(&data);

2
flight/OpenPilot/UAVObjects/attitudesettings.c
View File

@ -73,6 +73,8 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
// Initialize object fields to their default values // Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data); UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(AttitudeSettingsData)); memset(&data, 0, sizeof(AttitudeSettingsData));
data.Algorithm = 0;
data.UpdateRaw = 0;
data.UpdatePeriod = 500; data.UpdatePeriod = 500;
UAVObjSetInstanceData(obj, instId, &data); UAVObjSetInstanceData(obj, instId, &data);

8
flight/OpenPilot/UAVObjects/inc/ahrsstatus.h
View File

@ -33,7 +33,7 @@
#define AHRSSTATUS_H #define AHRSSTATUS_H
// Object constants // Object constants
#define AHRSSTATUS_OBJID 1556283776U #define AHRSSTATUS_OBJID 1048419880U
#define AHRSSTATUS_NAME "AhrsStatus" #define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta" #define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1 #define AHRSSTATUS_ISSINGLEINST 1
@ -58,7 +58,7 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t SerialNumber[25]; uint8_t SerialNumber[25];
uint8_t CommErrors[6]; uint8_t CommErrors[4];
uint8_t HomeSet; uint8_t HomeSet;
uint8_t CalibrationSet; uint8_t CalibrationSet;
@ -70,9 +70,9 @@ typedef struct {
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25 #define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
// Field CommErrors information // Field CommErrors information
/* Array element names for field CommErrors */ /* 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 */ /* Number of elements for field CommErrors */
#define AHRSSTATUS_COMMERRORS_NUMELEM 6 #define AHRSSTATUS_COMMERRORS_NUMELEM 4
// Field HomeSet information // Field HomeSet information
/* Enumeration options for field HomeSet */ /* Enumeration options for field HomeSet */
typedef enum { AHRSSTATUS_HOMESET_FALSE=0, AHRSSTATUS_HOMESET_TRUE=1 } AhrsStatusHomeSetOptions; typedef enum { AHRSSTATUS_HOMESET_FALSE=0, AHRSSTATUS_HOMESET_TRUE=1 } AhrsStatusHomeSetOptions;

10
flight/OpenPilot/UAVObjects/inc/attitudesettings.h
View File

@ -33,7 +33,7 @@
#define ATTITUDESETTINGS_H #define ATTITUDESETTINGS_H
// Object constants // Object constants
#define ATTITUDESETTINGS_OBJID 3446368842U #define ATTITUDESETTINGS_OBJID 2356162226U
#define ATTITUDESETTINGS_NAME "AttitudeSettings" #define ATTITUDESETTINGS_NAME "AttitudeSettings"
#define ATTITUDESETTINGS_METANAME "AttitudeSettingsMeta" #define ATTITUDESETTINGS_METANAME "AttitudeSettingsMeta"
#define ATTITUDESETTINGS_ISSINGLEINST 1 #define ATTITUDESETTINGS_ISSINGLEINST 1
@ -57,11 +57,19 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t Algorithm;
uint8_t UpdateRaw;
int32_t UpdatePeriod; int32_t UpdatePeriod;
} __attribute__((packed)) AttitudeSettingsData; } __attribute__((packed)) AttitudeSettingsData;
// Field information // 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 // Field UpdatePeriod information

55
flight/PiOS/Common/pios_opahrs.c
View File

@ -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); 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) { if (opahrs_msg_txrx((const uint8_t *)&link_tx, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
return OPAHRS_RESULT_FAILED; 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) { switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY: case OPAHRS_MSG_LINK_STATE_BUSY:
/* Wait for a small delay and retry */ /* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS); vTaskDelay(40 / portTICK_RATE_MS);
continue; continue;
case OPAHRS_MSG_LINK_STATE_INACTIVE: case OPAHRS_MSG_LINK_STATE_INACTIVE:
case OPAHRS_MSG_LINK_STATE_READY: 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); 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) enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req)
{ {
struct opahrs_msg_v1 rsp; 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); 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; enum opahrs_result rc;
if (!req) { if (!req) {
@ -299,7 +276,7 @@ enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
} }
/* Make up an attituderaw request */ /* 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 */ /* Send the message until it is received */
rc = opahrs_msg_v1_send_req (req); 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 rc;
} }
return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_GPS, &rsp); return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_UPDATE, 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);
} }
enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp) enum opahrs_result PIOS_OPAHRS_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp)

4
flight/PiOS/inc/pios_opahrs.h
View File

@ -41,11 +41,9 @@ enum opahrs_result {
extern void PIOS_OPAHRS_Init(void); extern void PIOS_OPAHRS_Init(void);
extern enum opahrs_result PIOS_OPAHRS_Sync(struct opahrs_msg_v1 *rsp); 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_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_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_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_SetGetCalibration(struct opahrs_msg_v1 *req, struct opahrs_msg_v1 *rsp);
extern enum opahrs_result PIOS_OPAHRS_resync(void); extern enum opahrs_result PIOS_OPAHRS_resync(void);

54
flight/PiOS/inc/pios_opahrs_proto.h
View File

@ -184,31 +184,27 @@ struct opahrs_msg_v1_req_reset {
struct opahrs_msg_v1_req_serial { struct opahrs_msg_v1_req_serial {
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_req_altitude {
float altitude;
float pressure;
float temperature;
} __attribute__((__packed__));
struct opahrs_msg_v1_req_north { struct opahrs_msg_v1_req_north {
float Be[3]; float Be[3];
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_req_gps { struct opahrs_msg_v1_req_update {
float latitude; struct {
float longitude; uint8_t updated;
float altitude; float NED[3];
float heading; float groundspeed;
float groundspeed; float heading;
uint8_t status; float quality;
} gps;
struct {
uint8_t updated;
float altitude;
} barometer;
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_req_attituderaw { struct opahrs_msg_v1_req_attituderaw {
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_req_attitude {
} __attribute__((__packed__));
struct opahrs_msg_v1_req_calibration { struct opahrs_msg_v1_req_calibration {
uint8_t measure_var; uint8_t measure_var;
uint16_t accel_bias[3]; 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_sync sync;
struct opahrs_msg_v1_req_reset reset; struct opahrs_msg_v1_req_reset reset;
struct opahrs_msg_v1_req_serial serial; 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_north north;
struct opahrs_msg_v1_req_gps gps;
struct opahrs_msg_v1_req_attituderaw attituderaw; struct opahrs_msg_v1_req_attituderaw attituderaw;
struct opahrs_msg_v1_req_attitude attitude;
struct opahrs_msg_v1_req_calibration calibration; struct opahrs_msg_v1_req_calibration calibration;
} __attribute__((__packed__)); } __attribute__((__packed__));
@ -247,15 +241,9 @@ struct opahrs_msg_v1_rsp_serial {
uint8_t serial_bcd[25]; uint8_t serial_bcd[25];
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_rsp_altitude {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_north { struct opahrs_msg_v1_rsp_north {
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_rsp_gps {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_attituderaw { struct opahrs_msg_v1_rsp_attituderaw {
struct { struct {
int16_t x; int16_t x;
@ -286,7 +274,7 @@ struct opahrs_msg_v1_rsp_attituderaw {
} accels_filtered; } accels_filtered;
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_rsp_attitude { struct opahrs_msg_v1_rsp_update {
struct { struct {
float q1; float q1;
float q2; float q2;
@ -298,6 +286,8 @@ struct opahrs_msg_v1_rsp_attitude {
float pitch; float pitch;
float yaw; float yaw;
} euler; } euler;
float NED[3];
float Vel[3];
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_rsp_calibration { struct opahrs_msg_v1_rsp_calibration {
@ -311,11 +301,9 @@ struct opahrs_msg_v1_rsp_calibration {
union opahrs_msg_v1_rsp { union opahrs_msg_v1_rsp {
struct opahrs_msg_v1_rsp_sync sync; struct opahrs_msg_v1_rsp_sync sync;
struct opahrs_msg_v1_rsp_serial serial; 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_north north;
struct opahrs_msg_v1_rsp_gps gps;
struct opahrs_msg_v1_rsp_attituderaw attituderaw; 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; struct opahrs_msg_v1_rsp_calibration calibration;
} __attribute__((__packed__)); } __attribute__((__packed__));
@ -324,20 +312,16 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_REQ_SYNC, OPAHRS_MSG_V1_REQ_SYNC,
OPAHRS_MSG_V1_REQ_RESET, OPAHRS_MSG_V1_REQ_RESET,
OPAHRS_MSG_V1_REQ_SERIAL, OPAHRS_MSG_V1_REQ_SERIAL,
OPAHRS_MSG_V1_REQ_ALTITUDE,
OPAHRS_MSG_V1_REQ_NORTH, OPAHRS_MSG_V1_REQ_NORTH,
OPAHRS_MSG_V1_REQ_GPS, OPAHRS_MSG_V1_REQ_UPDATE,
OPAHRS_MSG_V1_REQ_ATTITUDERAW, OPAHRS_MSG_V1_REQ_ATTITUDERAW,
OPAHRS_MSG_V1_REQ_ATTITUDE,
OPAHRS_MSG_V1_REQ_CALIBRATION, OPAHRS_MSG_V1_REQ_CALIBRATION,
OPAHRS_MSG_V1_RSP_SYNC, OPAHRS_MSG_V1_RSP_SYNC,
OPAHRS_MSG_V1_RSP_SERIAL, OPAHRS_MSG_V1_RSP_SERIAL,
OPAHRS_MSG_V1_RSP_ALTITUDE,
OPAHRS_MSG_V1_RSP_NORTH, OPAHRS_MSG_V1_RSP_NORTH,
OPAHRS_MSG_V1_RSP_GPS, OPAHRS_MSG_V1_RSP_UPDATE,
OPAHRS_MSG_V1_RSP_ATTITUDERAW, OPAHRS_MSG_V1_RSP_ATTITUDERAW,
OPAHRS_MSG_V1_RSP_ATTITUDE,
OPAHRS_MSG_V1_RSP_CALIBRATION, OPAHRS_MSG_V1_RSP_CALIBRATION,
}; };

8
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -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>"; }; 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>"; }; 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>"; }; 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; }; 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>"; }; 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>"; }; 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>"; }; 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>"; }; 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>"; }; 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>"; }; 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>"; }; 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>"; }; 65B367EB121C2620003EAD18 /* attituderaw.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attituderaw.xml; sourceTree = "<group>"; };
@ -6668,14 +6669,14 @@
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
655268BC121FBD2900410C6E /* ahrscalibration.xml */, 655268BC121FBD2900410C6E /* ahrscalibration.xml */,
65B367E7121C2620003EAD18 /* ahrsstatus.xml */,
65B367E4121C2620003EAD18 /* actuatorcommand.xml */, 65B367E4121C2620003EAD18 /* actuatorcommand.xml */,
65B367E5121C2620003EAD18 /* actuatordesired.xml */, 65B367E5121C2620003EAD18 /* actuatordesired.xml */,
65B367E6121C2620003EAD18 /* actuatorsettings.xml */, 65B367E6121C2620003EAD18 /* actuatorsettings.xml */,
65B367E7121C2620003EAD18 /* ahrsstatus.xml */,
65B367E8121C2620003EAD18 /* altitudeactual.xml */,
65B367E9121C2620003EAD18 /* attitudeactual.xml */, 65B367E9121C2620003EAD18 /* attitudeactual.xml */,
65B367EA121C2620003EAD18 /* attitudedesired.xml */, 65B367EA121C2620003EAD18 /* attitudedesired.xml */,
65B367EB121C2620003EAD18 /* attituderaw.xml */, 65B367EB121C2620003EAD18 /* attituderaw.xml */,
65209A1812208B0600453371 /* baroaltitude.xml */,
65B367EC121C2620003EAD18 /* attitudesettings.xml */, 65B367EC121C2620003EAD18 /* attitudesettings.xml */,
65B367ED121C2620003EAD18 /* exampleobject1.xml */, 65B367ED121C2620003EAD18 /* exampleobject1.xml */,
65B367EE121C2620003EAD18 /* exampleobject2.xml */, 65B367EE121C2620003EAD18 /* exampleobject2.xml */,
@ -6684,6 +6685,7 @@
65B367F1121C2620003EAD18 /* flightsituationactual.xml */, 65B367F1121C2620003EAD18 /* flightsituationactual.xml */,
65B367F2121C2620003EAD18 /* flighttelemetrystats.xml */, 65B367F2121C2620003EAD18 /* flighttelemetrystats.xml */,
65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */, 65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */,
65209A1912208B0600453371 /* gpsposition.xml */,
657CEEAD121DB6C8007A1FBE /* homelocation.xml */, 657CEEAD121DB6C8007A1FBE /* homelocation.xml */,
65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */, 65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */,
65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */, 65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */,

4
ground/src/plugins/uavobjects/ahrsstatus.cpp
View File

@ -70,11 +70,9 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
SerialNumberElemNames.append("24"); SerialNumberElemNames.append("24");
fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) );
QStringList CommErrorsElemNames; QStringList CommErrorsElemNames;
CommErrorsElemNames.append("Attitude"); CommErrorsElemNames.append("Update");
CommErrorsElemNames.append("AttitudeRaw"); CommErrorsElemNames.append("AttitudeRaw");
CommErrorsElemNames.append("PositionActual");
CommErrorsElemNames.append("HomeLocation"); CommErrorsElemNames.append("HomeLocation");
CommErrorsElemNames.append("Altitude");
CommErrorsElemNames.append("Calibration"); CommErrorsElemNames.append("Calibration");
fields.append( new UAVObjectField(QString("CommErrors"), QString("count"), UAVObjectField::UINT8, CommErrorsElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("CommErrors"), QString("count"), UAVObjectField::UINT8, CommErrorsElemNames, QStringList()) );
QStringList HomeSetElemNames; QStringList HomeSetElemNames;

8
ground/src/plugins/uavobjects/ahrsstatus.h
View File

@ -44,7 +44,7 @@ public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 SerialNumber[25]; quint8 SerialNumber[25];
quint8 CommErrors[6]; quint8 CommErrors[4];
quint8 HomeSet; quint8 HomeSet;
quint8 CalibrationSet; quint8 CalibrationSet;
@ -56,9 +56,9 @@ public:
static const quint32 SERIALNUMBER_NUMELEM = 25; static const quint32 SERIALNUMBER_NUMELEM = 25;
// Field CommErrors information // Field CommErrors information
/* Array element names for field CommErrors */ /* 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 */ /* Number of elements for field CommErrors */
static const quint32 COMMERRORS_NUMELEM = 6; static const quint32 COMMERRORS_NUMELEM = 4;
// Field HomeSet information // Field HomeSet information
/* Enumeration options for field HomeSet */ /* Enumeration options for field HomeSet */
typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions; typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions;
@ -68,7 +68,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 1556283776U; static const quint32 OBJID = 1048419880U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0; static const bool ISSETTINGS = 0;

8
ground/src/plugins/uavobjects/ahrsstatus.py
View File

@ -74,13 +74,11 @@ _fields = [ \
uavobject.UAVObjectField( uavobject.UAVObjectField(
'CommErrors', 'CommErrors',
'B', 'B',
6, 4,
[ [
'Attitude', 'Update',
'AttitudeRaw', 'AttitudeRaw',
'PositionActual',
'HomeLocation', 'HomeLocation',
'Altitude',
'Calibration', 'Calibration',
], ],
{ {
@ -115,7 +113,7 @@ _fields = [ \
class AhrsStatus(uavobject.UAVObject): class AhrsStatus(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 1556283776 OBJID = 1048419880
NAME = "AhrsStatus" NAME = "AhrsStatus"
METANAME = "AhrsStatusMeta" METANAME = "AhrsStatusMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

13
ground/src/plugins/uavobjects/attitudesettings.cpp
View File

@ -42,6 +42,17 @@ AttitudeSettings::AttitudeSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTI
{ {
// Create fields // Create fields
QList<UAVObjectField*> 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; QStringList UpdatePeriodElemNames;
UpdatePeriodElemNames.append("0"); UpdatePeriodElemNames.append("0");
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::INT32, UpdatePeriodElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::INT32, UpdatePeriodElemNames, QStringList()) );
@ -78,6 +89,8 @@ UAVObject::Metadata AttitudeSettings::getDefaultMetadata()
*/ */
void AttitudeSettings::setDefaultFieldValues() void AttitudeSettings::setDefaultFieldValues()
{ {
data.Algorithm = 0;
data.UpdateRaw = 0;
data.UpdatePeriod = 500; data.UpdatePeriod = 500;
} }

10
ground/src/plugins/uavobjects/attitudesettings.h
View File

@ -43,16 +43,24 @@ class UAVOBJECTS_EXPORT AttitudeSettings: public UAVDataObject
public: public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 Algorithm;
quint8 UpdateRaw;
qint32 UpdatePeriod; qint32 UpdatePeriod;
} __attribute__((packed)) DataFields; } __attribute__((packed)) DataFields;
// Field information // 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 // Field UpdatePeriod information
// Constants // Constants
static const quint32 OBJID = 3446368842U; static const quint32 OBJID = 2356162226U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1; static const bool ISSETTINGS = 1;

25
ground/src/plugins/uavobjects/attitudesettings.py
View File

@ -37,6 +37,29 @@ from collections import namedtuple
# This is a list of instances of the data fields contained in this object # This is a list of instances of the data fields contained in this object
_fields = [ \ _fields = [ \
uavobject.UAVObjectField(
'Algorithm',
'b',
1,
[
'0',
],
{
'0' : 'INSGPS',
}
),
uavobject.UAVObjectField(
'UpdateRaw',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'UpdatePeriod', 'UpdatePeriod',
'i', 'i',
@ -52,7 +75,7 @@ _fields = [ \
class AttitudeSettings(uavobject.UAVObject): class AttitudeSettings(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 3446368842 OBJID = 2356162226
NAME = "AttitudeSettings" NAME = "AttitudeSettings"
METANAME = "AttitudeSettingsMeta" METANAME = "AttitudeSettingsMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

2
ground/src/shared/uavobjectdefinition/ahrsstatus.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="AhrsStatus" singleinstance="true" settings="false"> <object name="AhrsStatus" singleinstance="true" settings="false">
<field name="SerialNumber" units="n/a" type="uint8" elements="25"/> <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="HomeSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/> <field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>

2
ground/src/shared/uavobjectdefinition/attitudesettings.xml
View File

@ -1,5 +1,7 @@
<xml> <xml>
<object name="AttitudeSettings" singleinstance="true" settings="true"> <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"/> <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="500"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>