rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15: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 {
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:

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

@ -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);

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
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(AttitudeSettingsData));
data.Algorithm = 0;
data.UpdateRaw = 0;
data.UpdatePeriod = 500;
UAVObjSetInstanceData(obj, instId, &data);

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

@ -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;

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

@ -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

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);
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)

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

@ -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);

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 {
} __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,
};

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>"; };
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 */,

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

@ -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;

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

@ -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;

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

@ -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

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

@ -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;
}

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

@ -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;

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
_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

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

@ -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"/>

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

@ -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"/>