rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-118 AHRS: Added AHRSCalibration object and messages associated with that to AHRS. Also made AHRS run calibration only when requested. We will create a GCS plugin that runs through computing the calibration settings. To compute the variances on the AHRS set the measure variances to true, wait ten seconds, then set it to false to read it back.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1363 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-21 21:46:02 +00:00 committed by peabody124
parent cc4d16ed69
commit 8897c441fa
26 changed files with 1469 additions and 370 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

393
flight/AHRS/ahrs.c
View File

@ -174,6 +174,7 @@ void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void initialize_location();
void converge_insgps();
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
@ -181,17 +182,21 @@ void initialize_location();
* Public data. Used by both EKF and the sender
*/
//! Accelerometer variance after filter from OP or calibrate_sensors
float accel_var[3];
float accel_var[3] = {1,1,1};
//! Gyro variance after filter from OP or calibrate sensors
float gyro_var[3];
float gyro_var[3] = {1,1,1};
//! Accelerometer scale after calibration
float accel_scale[3] = {ACCEL_SCALE, ACCEL_SCALE, ACCEL_SCALE};
//! Gyro scale after calibration
float gyro_scale[3] = {GYRO_SCALE, GYRO_SCALE, GYRO_SCALE};
//! Magnetometer variance from OP or calibrate sensors
float mag_var[3];
float mag_var[3] = {1,1,1};
//! Accelerometer bias from OP or calibrate sensors
float accel_bias[3];
int16_t accel_bias[3] = {ACCEL_OFFSET, ACCEL_OFFSET, ACCEL_OFFSET};
//! Gyroscope bias term from OP or calibrate sensors
float gyro_bias[3];
int16_t gyro_bias[3] = {0,0,0};
//! Magnetometer bias (direction) from OP or calibrate sensors
float mag_bias[3];
int16_t mag_bias[3] = {0,0,0};
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[ADC_OVERSAMPLE+1];
//! Raw buffer that DMA data is dumped into
@ -210,7 +215,8 @@ double BaseECEF[3] = {0, 0, 0};
float Rne[3][3];
//! Current location in NED coordinates relative to base
float NED[3];
//! Current location in NED coordinates relative to base
//! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE;
/**
* @}
@ -223,10 +229,11 @@ float NED[3];
int main()
{
float gyro[3], accel[3], mag[3];
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0;
float vel[3] = {0,0,0};
uint32_t loop_ctr = 0;
ahrs_algorithm = INSGPS_Algo;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
@ -270,49 +277,12 @@ int main()
fir_coeffs[i] = 1;
fir_coeffs[ADC_OVERSAMPLE] = ADC_OVERSAMPLE;
pos[0] = 0;
pos[1] = 0;
pos[2] = 0;
BaroAlt = 0;
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
/******* Get initial estimate of gyro bias term *************/
// TODO: There needs to be a calibration mode, then this is received from the SD card
// otherwise if we reset in air during a snap, this will be all wrong
calibrate_sensors();
if(ahrs_algorithm == INSGPS_Algo) {
INSGPSInit();
// INS algo wants noise on magnetometer in unit length variance
float mag_length = mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] + mag_bias[2] * mag_bias[2];
float scaled_mag_var[3] = {mag_var[0] / mag_length, mag_var[1] / mag_length, mag_var[2] / mag_length};
INSSetMagVar(scaled_mag_var);
// Initialize the algorithm assuming stationary
float temp_var[3] = {10, 10, 10};
float temp_gyro[3] = {0, 0, 0};
INSSetGyroVar(temp_var); // ignore gyro's
for(int i = 0; i < 100; i++)
{
// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.
// This should be done directly but I'm too dumb.
float rpy[3];
Quaternion2RPY(Nav.q, rpy);
rpy[1] = attitude_data.euler.pitch = -atan2(accel_bias[0], accel_bias[2]) * 180 / M_PI;
rpy[0] = attitude_data.euler.roll = -atan2(accel_bias[1], accel_bias[2]) * 180 / M_PI;
RPY2Quaternion(rpy,Nav.q);
INSPrediction( temp_gyro, accel_bias, 1 / (float) EKF_RATE );
FullCorrection(mag,pos,vel,BaroAlt);
}
INSSetGyroBias(gyro_bias);
INSSetAccelVar(accel_var);
INSSetGyroVar(gyro_var);
// compute a data point and initialize INS
downsample_data();
converge_insgps();
}
/******************* Main EKF loop ****************************/
while (1) {
// Alive signal
@ -320,6 +290,12 @@ int main()
loop_ctr ++;
if(calibration_pending)
{
calibrate_sensors();
calibration_pending = FALSE;
}
// Get magnetic readings
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
@ -413,7 +389,7 @@ int main()
}
ahrs_state = AHRS_IDLE;
process_spi_request();
}
@ -455,40 +431,40 @@ void downsample_data()
accel_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
{
temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[1] ) * fir_coeffs[i];
accel_raw[0] += temp;
}
accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
accel_raw[1] += ( valid_data_buffer[2 + i * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
accel_data.filtered.x = (float) accel_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
accel_raw[1] += ( valid_data_buffer[2 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[0] ) * fir_coeffs[i];
accel_data.filtered.x = (float) accel_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[0];
// Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
accel_raw[2] += ( valid_data_buffer[4 + i * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
accel_raw[2] += ( valid_data_buffer[4 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[2] ) * fir_coeffs[i];
accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
gyro_raw[0] += ( valid_data_buffer[1 + i * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
gyro_raw[0] += ( valid_data_buffer[1 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[0] ) * fir_coeffs[i];
gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
gyro_raw[1] += ( valid_data_buffer[3 + i * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
gyro_raw[1] += ( valid_data_buffer[3 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[1] ) * fir_coeffs[i];
gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ )
gyro_raw[2] += ( valid_data_buffer[5 + i * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
gyro_raw[2] += ( valid_data_buffer[5 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[2] ) * fir_coeffs[i];
gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[2];
}
/**
@ -501,7 +477,9 @@ void downsample_data()
void calibrate_sensors() {
int i;
int16_t mag_raw[3];
// local biases for noise analysis
float accel_bias[3], gyro_bias[3], mag_bias[3];
// run few loops to get mean
gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
@ -522,6 +500,7 @@ void calibrate_sensors() {
mag_bias[2] += mag_raw[2];
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_bias[0] /= i;
gyro_bias[1] /= i;
@ -553,6 +532,7 @@ void calibrate_sensors() {
mag_var[1] += (mag_raw[1] - mag_bias[1]) * (mag_raw[1] - mag_bias[1]);
mag_var[2] += (mag_raw[2] - mag_bias[2]) * (mag_raw[2] - mag_bias[2]);
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_var[0] /= i;
gyro_var[1] /= i;
@ -562,7 +542,61 @@ void calibrate_sensors() {
accel_var[2] /= i;
mag_var[0] /= i;
mag_var[1] /= i;
mag_var[2] /= i;
mag_var[2] /= i;
float mag_length2 = mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] + mag_bias[2] * mag_bias[2];
mag_var[0] = mag_var[0] / mag_length2;
mag_var[1] = mag_var[1] / mag_length2;
mag_var[2] = mag_var[2] / mag_length2;
if(ahrs_algorithm == INSGPS_Algo)
converge_insgps();
}
/**
* @brief Quickly initialize INS assuming stationary and gravity is down
*
* Currently this is done iteratively but I'm sure it can be directly computed
* when I sit down and work it out
*/
void converge_insgps()
{
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {0, 0, 0};
INSGPSInit();
INSSetAccelVar(accel_var);
INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
float temp_var[3] = {10, 10, 10};
INSSetGyroVar(temp_var); // ignore gyro's
accel[0] = accel_data.filtered.x;
accel[1] = accel_data.filtered.y;
accel[2] = accel_data.filtered.z;
// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.
for(int i = 0; i < 50; i++)
{
// This should be done directly but I'm too dumb.
float rpy[3];
Quaternion2RPY(Nav.q, rpy);
rpy[1] = attitude_data.euler.pitch = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI;
rpy[0] = attitude_data.euler.roll = -atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
// Get magnetic readings
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag[0] = -mag_data.raw.axis[1];
mag[1] = -mag_data.raw.axis[0];
mag[2] = -mag_data.raw.axis[2];
RPY2Quaternion(rpy,Nav.q);
INSPrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
FullCorrection(mag,pos,vel,BaroAlt);
process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
}
INSSetGyroVar(gyro_var);
}
/**
@ -627,96 +661,149 @@ void process_spi_request(void)
//dump_spi_message(PIOS_COM_AUX, "+", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
switch (user_rx_v1.payload.user.t) {
case OPAHRS_MSG_V1_REQ_SYNC:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SYNC);
user_tx_v1.payload.user.v.rsp.sync.i_am_a_bootloader = FALSE;
user_tx_v1.payload.user.v.rsp.sync.hw_version = 1;
user_tx_v1.payload.user.v.rsp.sync.bl_version = 2;
user_tx_v1.payload.user.v.rsp.sync.fw_version = 3;
user_tx_v1.payload.user.v.rsp.sync.cookie = user_rx_v1.payload.user.v.req.sync.cookie;
dump_spi_message(PIOS_COM_AUX, "S", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V1_REQ_SERIAL:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SERIAL);
PIOS_SYS_SerialNumberGet((char *)&(user_tx_v1.payload.user.v.rsp.serial.serial_bcd));
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);
dump_spi_message(PIOS_COM_AUX, "N", (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];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y = mag_data.raw.axis[1];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.z = mag_data.raw.axis[2];
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x = gyro_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y = gyro_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z = gyro_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.x = gyro_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.y = gyro_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.z = gyro_data.filtered.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp = gyro_data.temp.xy;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp = gyro_data.temp.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.x = accel_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.y = accel_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.z = accel_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.x = accel_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.y = accel_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.z = accel_data.filtered.z;
case OPAHRS_MSG_V1_REQ_SYNC:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SYNC);
user_tx_v1.payload.user.v.rsp.sync.i_am_a_bootloader = FALSE;
user_tx_v1.payload.user.v.rsp.sync.hw_version = 1;
user_tx_v1.payload.user.v.rsp.sync.bl_version = 2;
user_tx_v1.payload.user.v.rsp.sync.fw_version = 3;
user_tx_v1.payload.user.v.rsp.sync.cookie = user_rx_v1.payload.user.v.req.sync.cookie;
dump_spi_message(PIOS_COM_AUX, "S", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V1_REQ_SERIAL:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SERIAL);
PIOS_SYS_SerialNumberGet((char *)&(user_tx_v1.payload.user.v.rsp.serial.serial_bcd));
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);
dump_spi_message(PIOS_COM_AUX, "N", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_CALIBRATION:
if(user_rx_v1.payload.user.v.req.calibration.measure_var)
calibration_pending = TRUE;
else {
accel_var[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0];
accel_var[1] = user_rx_v1.payload.user.v.req.calibration.accel_var[1];
accel_var[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
gyro_bias[0] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[0];
gyro_bias[1] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[1];
gyro_bias[2] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[2];
gyro_var[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
gyro_var[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
gyro_var[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
mag_var[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
mag_var[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
mag_var[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
INSSetAccelVar(accel_var);
float gyro_bias_ins[3] = {0,0,0};
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
}
accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
accel_scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
accel_scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
accel_scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
mag_bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
mag_bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
mag_bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
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));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
default:
break;
// echo back the values used
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_CALIBRATION);
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] = accel_var[0];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] = accel_var[1];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] = accel_var[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] = gyro_bias[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] = gyro_bias[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] = gyro_bias[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_var[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_var[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_var[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_var[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_var[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_var[2];
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];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y = mag_data.raw.axis[1];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.z = mag_data.raw.axis[2];
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x = gyro_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y = gyro_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z = gyro_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.x = gyro_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.y = gyro_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.z = gyro_data.filtered.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp = gyro_data.temp.xy;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp = gyro_data.temp.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.x = accel_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.y = accel_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.z = accel_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.x = accel_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.y = accel_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.z = accel_data.filtered.z;
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));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
default:
break;
}
/* Finished processing the received message, requeue it */
memset(&user_rx_v1, 0xAA, sizeof(user_rx_v1));
lfsm_user_set_rx_v1 (&user_rx_v1);

1
flight/OpenPilot/Makefile
View File

@ -164,6 +164,7 @@ SRC += $(OPUAVOBJ)/attitudedesired.c
SRC += $(OPUAVOBJ)/stabilizationsettings.c
SRC += $(OPUAVOBJ)/ahrsstatus.c
SRC += $(OPUAVOBJ)/baroaltitude.c
SRC += $(OPUAVOBJ)/ahrscalibration.c
SRC += $(OPUAVOBJ)/attitudeactual.c
SRC += $(OPUAVOBJ)/attitudesettings.c
SRC += $(OPUAVOBJ)/flightsituationactual.c

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

@ -60,6 +60,7 @@
#include "stdbool.h"
#include "positionactual.h"
#include "homelocation.h"
#include "ahrscalibration.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"
@ -78,9 +79,11 @@ static void ahrscommsTask(void* parameters);
static void load_baro_altitude(struct opahrs_msg_v1_req_altitude * altitude);
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 bool BaroAltitudeIsUpdatedFlag = false;
static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
@ -100,7 +103,12 @@ static void HomeLocationUpdatedCb(UAVObjEvent * ev)
HomeLocationIsUpdatedFlag = true;
}
static bool AHRSKnowsHome = FALSE;
static bool AHRSCalibrationIsUpdatedFlag = false;
static void AHRSCalibrationUpdatedCb(UAVObjEvent * ev)
{
AHRSCalibrationIsUpdatedFlag = true;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -110,6 +118,7 @@ int32_t AHRSCommsInitialize(void)
BaroAltitudeConnectCallback(BaroAltitudeUpdatedCb);
PositionActualConnectCallback(PositionActualUpdatedCb);
HomeLocationConnectCallback(HomeLocationUpdatedCb);
AHRSCalibrationConnectCallback(AHRSCalibrationUpdatedCb);
PIOS_OPAHRS_Init();
@ -119,7 +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;
static uint16_t attitude_errors = 0, attituderaw_errors = 0, position_errors = 0,
home_errors = 0, altitude_errors = 0, calibration_errors;
/**
* Module thread, should not return.
@ -131,11 +141,15 @@ static void ahrscommsTask(void* parameters)
// Main task loop
while (1) {
struct opahrs_msg_v1 rsp;
AhrsStatusData data;
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
/* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */
AHRSKnowsHome = FALSE;
AhrsStatusGet(&data);
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
/* Spin here until we're in sync */
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
@ -178,16 +192,16 @@ static void ahrscommsTask(void* parameters)
}
if (BaroAltitudeIsUpdatedFlag) {
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 req;
load_baro_altitude(&(req.payload.user.v.req.altitude));
if ((result = PIOS_OPAHRS_SetBaroAltitude(&req)) == OPAHRS_RESULT_OK) {
BaroAltitudeIsUpdatedFlag = false;
} else {
/* Comms error */
altitude_errors++;
break;
}
load_baro_altitude(&(req.payload.user.v.req.altitude));
if ((result = PIOS_OPAHRS_SetBaroAltitude(&req)) == OPAHRS_RESULT_OK) {
BaroAltitudeIsUpdatedFlag = false;
} else {
/* Comms error */
altitude_errors++;
break;
}
}
if (PositionActualIsUpdatedFlag) {
@ -203,27 +217,102 @@ static void ahrscommsTask(void* parameters)
}
}
if (HomeLocationIsUpdatedFlag || !AHRSKnowsHome) {
AhrsStatusGet(&data);
if (HomeLocationIsUpdatedFlag || (data.HomeSet == AHRSSTATUS_HOMESET_FALSE)) {
struct opahrs_msg_v1 req;
load_magnetic_north(&(req.payload.user.v.req.north));
if ((result = PIOS_OPAHRS_SetMagNorth(&req)) == OPAHRS_RESULT_OK) {
HomeLocationIsUpdatedFlag = false;
AHRSKnowsHome = TRUE;
data.HomeSet = AHRSSTATUS_HOMESET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
PIOS_LED_Off(LED2);
home_errors++;
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
AhrsStatusSet(&data);
break;
}
}
AhrsStatusGet(&data);
if (AHRSCalibrationIsUpdatedFlag || (data.CalibrationSet == AHRSSTATUS_CALIBRATIONSET_FALSE))
{
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
load_calibration(&(req.payload.user.v.req.calibration));
if(( result = PIOS_OPAHRS_SetGetCalibration(&req,&rsp) ) == OPAHRS_RESULT_OK ) {
update_calibration(&(rsp.payload.user.v.rsp.calibration));
AHRSCalibrationIsUpdatedFlag = false;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
AhrsStatusSet(&data);
} else {
/* Comms error */
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
AhrsStatusSet(&data);
calibration_errors++;
break;
}
}
/* Wait for the next update interval */
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
}
}
}
static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
calibration->measure_var = data.measure_var;
calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2];
calibration->accel_scale[0] = data.accel_scale[0];
calibration->accel_scale[1] = data.accel_scale[1];
calibration->accel_scale[2] = data.accel_scale[2];
calibration->accel_var[0] = data.accel_var[0];
calibration->accel_var[1] = data.accel_var[1];
calibration->accel_var[2] = data.accel_var[2];
calibration->gyro_bias[0] = data.gyro_bias[0];
calibration->gyro_bias[1] = data.gyro_bias[1];
calibration->gyro_bias[2] = data.gyro_bias[2];
calibration->gyro_scale[0] = data.gyro_scale[0];
calibration->gyro_scale[1] = data.gyro_scale[1];
calibration->gyro_scale[2] = data.gyro_scale[2];
calibration->gyro_var[0] = data.gyro_var[0];
calibration->gyro_var[1] = data.gyro_var[1];
calibration->gyro_var[2] = data.gyro_var[2];
calibration->mag_bias[0] = data.mag_bias[0];
calibration->mag_bias[1] = data.mag_bias[1];
calibration->mag_bias[2] = data.mag_bias[2];
calibration->mag_var[0] = data.mag_var[0];
calibration->mag_var[1] = data.mag_var[1];
calibration->mag_var[2] = data.mag_var[2];
}
static void update_calibration(struct opahrs_msg_v1_rsp_calibration * calibration)
{
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
data.accel_var[0] = calibration->accel_var[0];
data.accel_var[1] = calibration->accel_var[1];
data.accel_var[2] = calibration->accel_var[2];
data.gyro_var[0] = calibration->gyro_var[0];
data.gyro_var[1] = calibration->gyro_var[1];
data.gyro_var[2] = calibration->gyro_var[2];
data.mag_var[0] = calibration->mag_var[0];
data.mag_var[1] = calibration->mag_var[1];
data.mag_var[2] = calibration->mag_var[2];
AHRSCalibrationSet(&data);
}
static void load_magnetic_north(struct opahrs_msg_v1_req_north * mag_north)
{
HomeLocationData data;
@ -337,6 +426,7 @@ static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
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);
}

127
flight/OpenPilot/UAVObjects/ahrscalibration.c Normal file
View File

@ -0,0 +1,127 @@
/**
******************************************************************************
*
* @file ahrscalibration.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the AHRSCalibration object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: ahrscalibration.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "ahrscalibration.h"
// Private variables
static UAVObjHandle handle;
// Private functions
static void setDefaults(UAVObjHandle obj, uint16_t instId);
/**
* Initialize object.
* \return 0 Success
* \return -1 Failure
*/
int32_t AHRSCalibrationInitialize()
{
// Register object with the object manager
handle = UAVObjRegister(AHRSCALIBRATION_OBJID, AHRSCALIBRATION_NAME, AHRSCALIBRATION_METANAME, 0,
AHRSCALIBRATION_ISSINGLEINST, AHRSCALIBRATION_ISSETTINGS, AHRSCALIBRATION_NUMBYTES, &setDefaults);
// Done
if (handle != 0)
{
return 0;
}
else
{
return -1;
}
}
/**
* Initialize object fields and metadata with the default values.
* If a default value is not specified the object fields
* will be initialized to zero.
*/
static void setDefaults(UAVObjHandle obj, uint16_t instId)
{
AHRSCalibrationData data;
UAVObjMetadata metadata;
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(AHRSCalibrationData));
data.measure_var = 1;
data.accel_bias[0] = -2048;
data.accel_bias[1] = -2048;
data.accel_bias[2] = -2048;
data.accel_scale[0] = 0.012;
data.accel_scale[1] = 0.012;
data.accel_scale[2] = 0.012;
data.accel_var[0] = 1;
data.accel_var[1] = 1;
data.accel_var[2] = 1;
data.gyro_bias[0] = -1675;
data.gyro_bias[1] = -1675;
data.gyro_bias[2] = -1675;
data.gyro_scale[0] = 0.007;
data.gyro_scale[1] = 0.007;
data.gyro_scale[2] = 0.007;
data.gyro_var[0] = 1;
data.gyro_var[1] = 1;
data.gyro_var[2] = 1;
data.mag_bias[0] = 0;
data.mag_bias[1] = 0;
data.mag_bias[2] = 0;
data.mag_var[0] = 1;
data.mag_var[1] = 1;
data.mag_var[2] = 1;
UAVObjSetInstanceData(obj, instId, &data);
// Initialize object metadata to their default values
metadata.access = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.telemetryAcked = 1;
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
metadata.telemetryUpdatePeriod = 0;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
metadata.loggingUpdatePeriod = 0;
UAVObjSetMetadata(obj, &metadata);
}
/**
* Get object handle
*/
UAVObjHandle AHRSCalibrationHandle()
{
return handle;
}

29
flight/OpenPilot/UAVObjects/homelocation.c
View File

@ -73,17 +73,36 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(HomeLocationData));
data.Set = -1;
data.Latitude = 0;
data.Longitude = 0;
data.Altitude = 0;
data.ECEF[0] = 0;
data.ECEF[1] = 0;
data.ECEF[2] = 0;
data.RNE[0] = 0;
data.RNE[1] = 0;
data.RNE[2] = 0;
data.RNE[3] = 0;
data.RNE[4] = 0;
data.RNE[5] = 0;
data.RNE[6] = 0;
data.RNE[7] = 0;
data.RNE[8] = 0;
data.Be[0] = 0;
data.Be[1] = 0;
data.Be[2] = 0;
UAVObjSetInstanceData(obj, instId, &data);
// Initialize object metadata to their default values
metadata.access = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = 10000;
metadata.gcsTelemetryAcked = 0;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
metadata.telemetryAcked = 1;
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
metadata.telemetryUpdatePeriod = 0;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
metadata.loggingUpdatePeriod = 0;

122
flight/OpenPilot/UAVObjects/inc/ahrscalibration.h Normal file
View File

@ -0,0 +1,122 @@
/**
******************************************************************************
*
* @file ahrscalibration.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Implementation of the AHRSCalibration object. This file has been
* automatically generated by the UAVObjectGenerator.
*
* @note Object definition file: ahrscalibration.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRSCALIBRATION_H
#define AHRSCALIBRATION_H
// Object constants
#define AHRSCALIBRATION_OBJID 3497014596U
#define AHRSCALIBRATION_NAME "AHRSCalibration"
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
#define AHRSCALIBRATION_ISSINGLEINST 1
#define AHRSCALIBRATION_ISSETTINGS 1
#define AHRSCALIBRATION_NUMBYTES sizeof(AHRSCalibrationData)
// Object access macros
#define AHRSCalibrationGet(dataOut) UAVObjGetData(AHRSCalibrationHandle(), dataOut)
#define AHRSCalibrationSet(dataIn) UAVObjSetData(AHRSCalibrationHandle(), dataIn)
#define AHRSCalibrationInstGet(instId, dataOut) UAVObjGetInstanceData(AHRSCalibrationHandle(), instId, dataOut)
#define AHRSCalibrationInstSet(instId, dataIn) UAVObjSetInstanceData(AHRSCalibrationHandle(), instId, dataIn)
#define AHRSCalibrationConnectQueue(queue) UAVObjConnectQueue(AHRSCalibrationHandle(), queue, EV_MASK_ALL_UPDATES)
#define AHRSCalibrationConnectCallback(cb) UAVObjConnectCallback(AHRSCalibrationHandle(), cb, EV_MASK_ALL_UPDATES)
#define AHRSCalibrationCreateInstance() UAVObjCreateInstance(AHRSCalibrationHandle())
#define AHRSCalibrationRequestUpdate() UAVObjRequestUpdate(AHRSCalibrationHandle())
#define AHRSCalibrationRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AHRSCalibrationHandle(), instId)
#define AHRSCalibrationUpdated() UAVObjUpdated(AHRSCalibrationHandle())
#define AHRSCalibrationInstUpdated(instId) UAVObjUpdated(AHRSCalibrationHandle(), instId)
#define AHRSCalibrationGetMetadata(dataOut) UAVObjGetMetadata(AHRSCalibrationHandle(), dataOut)
#define AHRSCalibrationSetMetadata(dataIn) UAVObjSetMetadata(AHRSCalibrationHandle(), dataIn)
// Object data
typedef struct {
uint8_t measure_var;
int16_t accel_bias[3];
float accel_scale[3];
float accel_var[3];
int16_t gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
float mag_bias[3];
float mag_var[3];
} __attribute__((packed)) AHRSCalibrationData;
// Field information
// Field measure_var information
/* Enumeration options for field measure_var */
typedef enum { AHRSCALIBRATION_MEASURE_VAR_FALSE=0, AHRSCALIBRATION_MEASURE_VAR_TRUE=1 } AHRSCalibrationmeasure_varOptions;
// Field accel_bias information
/* Array element names for field accel_bias */
typedef enum { AHRSCALIBRATION_ACCEL_BIAS_X=0, AHRSCALIBRATION_ACCEL_BIAS_Y=1, AHRSCALIBRATION_ACCEL_BIAS_Z=2 } AHRSCalibrationaccel_biasElem;
/* Number of elements for field accel_bias */
#define AHRSCALIBRATION_ACCEL_BIAS_NUMELEM 3
// Field accel_scale information
/* Array element names for field accel_scale */
typedef enum { AHRSCALIBRATION_ACCEL_SCALE_X=0, AHRSCALIBRATION_ACCEL_SCALE_Y=1, AHRSCALIBRATION_ACCEL_SCALE_Z=2 } AHRSCalibrationaccel_scaleElem;
/* Number of elements for field accel_scale */
#define AHRSCALIBRATION_ACCEL_SCALE_NUMELEM 3
// Field accel_var information
/* Array element names for field accel_var */
typedef enum { AHRSCALIBRATION_ACCEL_VAR_X=0, AHRSCALIBRATION_ACCEL_VAR_Y=1, AHRSCALIBRATION_ACCEL_VAR_Z=2 } AHRSCalibrationaccel_varElem;
/* Number of elements for field accel_var */
#define AHRSCALIBRATION_ACCEL_VAR_NUMELEM 3
// Field gyro_bias information
/* Array element names for field gyro_bias */
typedef enum { AHRSCALIBRATION_GYRO_BIAS_X=0, AHRSCALIBRATION_GYRO_BIAS_Y=1, AHRSCALIBRATION_GYRO_BIAS_Z=2 } AHRSCalibrationgyro_biasElem;
/* Number of elements for field gyro_bias */
#define AHRSCALIBRATION_GYRO_BIAS_NUMELEM 3
// Field gyro_scale information
/* Array element names for field gyro_scale */
typedef enum { AHRSCALIBRATION_GYRO_SCALE_X=0, AHRSCALIBRATION_GYRO_SCALE_Y=1, AHRSCALIBRATION_GYRO_SCALE_Z=2 } AHRSCalibrationgyro_scaleElem;
/* Number of elements for field gyro_scale */
#define AHRSCALIBRATION_GYRO_SCALE_NUMELEM 3
// Field gyro_var information
/* Array element names for field gyro_var */
typedef enum { AHRSCALIBRATION_GYRO_VAR_X=0, AHRSCALIBRATION_GYRO_VAR_Y=1, AHRSCALIBRATION_GYRO_VAR_Z=2 } AHRSCalibrationgyro_varElem;
/* Number of elements for field gyro_var */
#define AHRSCALIBRATION_GYRO_VAR_NUMELEM 3
// Field mag_bias information
/* Array element names for field mag_bias */
typedef enum { AHRSCALIBRATION_MAG_BIAS_X=0, AHRSCALIBRATION_MAG_BIAS_Y=1, AHRSCALIBRATION_MAG_BIAS_Z=2 } AHRSCalibrationmag_biasElem;
/* Number of elements for field mag_bias */
#define AHRSCALIBRATION_MAG_BIAS_NUMELEM 3
// Field mag_var information
/* Array element names for field mag_var */
typedef enum { AHRSCALIBRATION_MAG_VAR_X=0, AHRSCALIBRATION_MAG_VAR_Y=1, AHRSCALIBRATION_MAG_VAR_Z=2 } AHRSCalibrationmag_varElem;
/* Number of elements for field mag_var */
#define AHRSCALIBRATION_MAG_VAR_NUMELEM 3
// Generic interface functions
int32_t AHRSCalibrationInitialize();
UAVObjHandle AHRSCalibrationHandle();
#endif // AHRSCALIBRATION_H

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

@ -33,7 +33,7 @@
#define AHRSSTATUS_H
// Object constants
#define AHRSSTATUS_OBJID 2618706832U
#define AHRSSTATUS_OBJID 1556283776U
#define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1
@ -58,7 +58,9 @@
// Object data
typedef struct {
uint8_t SerialNumber[25];
uint8_t CommErrors[5];
uint8_t CommErrors[6];
uint8_t HomeSet;
uint8_t CalibrationSet;
} __attribute__((packed)) AhrsStatusData;
@ -68,9 +70,15 @@ 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 } AhrsStatusCommErrorsElem;
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;
/* Number of elements for field CommErrors */
#define AHRSSTATUS_COMMERRORS_NUMELEM 5
#define AHRSSTATUS_COMMERRORS_NUMELEM 6
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { AHRSSTATUS_HOMESET_FALSE=0, AHRSSTATUS_HOMESET_TRUE=1 } AhrsStatusHomeSetOptions;
// Field CalibrationSet information
/* Enumeration options for field CalibrationSet */
typedef enum { AHRSSTATUS_CALIBRATIONSET_FALSE=0, AHRSSTATUS_CALIBRATIONSET_TRUE=1 } AhrsStatusCalibrationSetOptions;
// Generic interface functions

4
flight/OpenPilot/UAVObjects/inc/homelocation.h
View File

@ -33,11 +33,11 @@
#define HOMELOCATION_H
// Object constants
#define HOMELOCATION_OBJID 576787928U
#define HOMELOCATION_OBJID 4244719376U
#define HOMELOCATION_NAME "HomeLocation"
#define HOMELOCATION_METANAME "HomeLocationMeta"
#define HOMELOCATION_ISSINGLEINST 1
#define HOMELOCATION_ISSETTINGS 0
#define HOMELOCATION_ISSETTINGS 1
#define HOMELOCATION_NUMBYTES sizeof(HomeLocationData)
// Object access macros

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

@ -31,6 +31,7 @@
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "actuatorsettings.h"
#include "ahrscalibration.h"
#include "ahrsstatus.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
@ -68,6 +69,7 @@ void UAVObjectsInitializeAll()
ActuatorCommandInitialize();
ActuatorDesiredInitialize();
ActuatorSettingsInitialize();
AHRSCalibrationInitialize();
AhrsStatusInitialize();
AttitudeActualInitialize();
AttitudeDesiredInitialize();

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

@ -37,8 +37,8 @@
#include "pios_opahrs.h"
/**
* Initialise the OpenPilot AHRS
*/
* Initialise the OpenPilot AHRS
*/
void PIOS_OPAHRS_Init(void)
{
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8);
@ -47,7 +47,7 @@ void PIOS_OPAHRS_Init(void)
static int32_t opahrs_msg_txrx (const uint8_t * tx, uint8_t * rx, uint32_t len)
{
int32_t rc;
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
vTaskDelay(20 / portTICK_RATE_MS);
rc = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, tx, rx, len, NULL);
@ -57,89 +57,90 @@ static int32_t opahrs_msg_txrx (const uint8_t * tx, uint8_t * rx, uint32_t len)
static enum opahrs_result opahrs_msg_v1_send_req (const struct opahrs_msg_v1 * req)
{
int32_t rc;
struct opahrs_msg_v1 link_rx;
for (uint8_t retries = 0; retries < 20; retries++) {
struct opahrs_msg_v1 * rsp = &link_rx;
if (opahrs_msg_txrx((const uint8_t *)req, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
if ((rc = opahrs_msg_txrx((const uint8_t *)req, (uint8_t *)rsp, sizeof(*rsp))) < 0) {
return OPAHRS_RESULT_FAILED;
}
/* Make sure we got a sane response by checking the magic */
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) ||
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
return OPAHRS_RESULT_FAILED;
}
switch (rsp->head.type) {
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
case OPAHRS_MSG_LINK_STATE_INACTIVE:
/* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS);
continue;
case OPAHRS_MSG_LINK_STATE_READY:
/* Peer was ready when we Tx'd so they have now Rx'd our message */
return OPAHRS_RESULT_OK;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
/* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS);
continue;
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
case OPAHRS_MSG_LINK_STATE_INACTIVE:
/* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS);
continue;
case OPAHRS_MSG_LINK_STATE_READY:
/* Peer was ready when we Tx'd so they have now Rx'd our message */
return OPAHRS_RESULT_OK;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
/* Wait for a small delay and retry */
vTaskDelay(50 / portTICK_RATE_MS);
continue;
}
}
return OPAHRS_RESULT_TIMEOUT;
}
static enum opahrs_result opahrs_msg_v1_recv_rsp (enum opahrs_msg_v1_tag tag, struct opahrs_msg_v1 * rsp)
{
struct opahrs_msg_v1 link_tx;
opahrs_msg_v1_init_link_tx(&link_tx, OPAHRS_MSG_LINK_TAG_NOP);
for (uint8_t retries = 0; retries < 20; retries++) {
if (opahrs_msg_txrx((const uint8_t *)&link_tx, (uint8_t *)rsp, sizeof(*rsp)) < 0) {
return OPAHRS_RESULT_FAILED;
}
/* Make sure we got a sane response by checking the magic */
if ((rsp->head.magic != OPAHRS_MSG_MAGIC_HEAD) ||
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
(rsp->tail.magic != OPAHRS_MSG_MAGIC_TAIL)) {
return OPAHRS_RESULT_FAILED;
}
switch (rsp->head.type) {
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
/* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS);
continue;
case OPAHRS_MSG_LINK_STATE_INACTIVE:
case OPAHRS_MSG_LINK_STATE_READY:
/* somehow, we've missed our response */
return OPAHRS_RESULT_FAILED;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
/* This isn't the type we expected */
return OPAHRS_RESULT_FAILED;
break;
case OPAHRS_MSG_TYPE_USER_V1:
if (rsp->payload.user.t == tag) {
return OPAHRS_RESULT_OK;
} else {
return OPAHRS_RESULT_FAILED;
}
break;
case OPAHRS_MSG_TYPE_LINK:
switch (rsp->payload.link.state) {
case OPAHRS_MSG_LINK_STATE_BUSY:
/* Wait for a small delay and retry */
vTaskDelay(20 / portTICK_RATE_MS);
continue;
case OPAHRS_MSG_LINK_STATE_INACTIVE:
case OPAHRS_MSG_LINK_STATE_READY:
/* somehow, we've missed our response */
return OPAHRS_RESULT_FAILED;
}
break;
case OPAHRS_MSG_TYPE_USER_V0:
/* This isn't the type we expected */
return OPAHRS_RESULT_FAILED;
break;
case OPAHRS_MSG_TYPE_USER_V1:
if (rsp->payload.user.t == tag) {
return OPAHRS_RESULT_OK;
} else {
return OPAHRS_RESULT_FAILED;
}
break;
}
}
return OPAHRS_RESULT_TIMEOUT;
}
@ -147,21 +148,21 @@ enum opahrs_result PIOS_OPAHRS_GetSerial(struct opahrs_msg_v1 *rsp)
{
struct opahrs_msg_v1 req;
enum opahrs_result rc;
if (!rsp) {
return -1;
}
/* Make up a serial number request */
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_SERIAL);
/* 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_SERIAL, rsp);
}
@ -169,22 +170,22 @@ enum opahrs_result PIOS_OPAHRS_resync(void)
{
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
enum opahrs_result rc = OPAHRS_RESULT_FAILED;
opahrs_msg_v1_init_link_tx(&req, OPAHRS_MSG_LINK_TAG_NOP);
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
vTaskDelay(20 / portTICK_RATE_MS);
for (uint32_t i = 0; i < sizeof(req); i++) {
/* Tx a shortened (by one byte) message to walk through all byte positions */
opahrs_msg_v1_init_rx(&rsp);
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *)&req, (uint8_t *)&rsp, sizeof(req)-1, NULL);
/* Good magic means we're sync'd */
if ((rsp.head.magic == OPAHRS_MSG_MAGIC_HEAD) &&
(rsp.tail.magic == OPAHRS_MSG_MAGIC_TAIL)) {
(rsp.tail.magic == OPAHRS_MSG_MAGIC_TAIL)) {
/* We need to shift out one more byte to compensate for the short tx */
PIOS_SPI_TransferByte(PIOS_OPAHRS_SPI, 0x00);
rc = OPAHRS_RESULT_OK;
@ -192,10 +193,10 @@ enum opahrs_result PIOS_OPAHRS_resync(void)
}
vTaskDelay(10 / portTICK_RATE_MS);
}
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
//vTaskDelay(5 / portTICK_RATE_MS);
return rc;
}
@ -203,22 +204,22 @@ extern enum opahrs_result PIOS_OPAHRS_Sync(struct opahrs_msg_v1 *rsp)
{
struct opahrs_msg_v1 req;
enum opahrs_result rc;
if (!rsp) {
return -1;
}
/* Make up a sync request */
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_SYNC);
req.payload.user.v.req.sync.cookie = 0xDEADBEEF;
/* 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_SYNC, rsp);
}
@ -226,21 +227,21 @@ enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp)
{
struct opahrs_msg_v1 req;
enum opahrs_result rc;
if (!rsp) {
return -1;
}
/* Make up an attituderaw request */
opahrs_msg_v1_init_user_tx (&req, OPAHRS_MSG_V1_REQ_ATTITUDERAW);
/* 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_ATTITUDERAW, rsp);
}
@ -248,21 +249,21 @@ 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);
}
@ -314,24 +315,46 @@ 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 rc;
if (!req) {
return -1;
}
/* Make up an attituderaw request */
opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_CALIBRATION);
/* 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_CALIBRATION, rsp);
}
#endif /* PIOS_INCLUDE_OPAHRS */
/**

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

@ -46,6 +46,7 @@ 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);
#endif /* PIOS_OPAHRS_H */

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

@ -209,6 +209,17 @@ struct opahrs_msg_v1_req_attituderaw {
struct opahrs_msg_v1_req_attitude {
} __attribute__((__packed__));
struct opahrs_msg_v1_req_calibration {
uint8_t measure_var;
uint16_t accel_bias[3];
float accel_scale[3];
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
uint16_t mag_bias[3];
float mag_var[3];
} __attribute__((__packed__));
union opahrs_msg_v1_req {
struct opahrs_msg_v1_req_nop nop;
@ -216,10 +227,11 @@ union opahrs_msg_v1_req {
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_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_attitude attitude;
struct opahrs_msg_v1_req_calibration calibration;
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_sync {
@ -288,14 +300,23 @@ struct opahrs_msg_v1_rsp_attitude {
} euler;
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_calibration {
uint8_t measure_var;
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_var[3];
float mag_var[3];
} __attribute__((__packed__));
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_altitude gps;
struct opahrs_msg_v1_rsp_calibration calibration;
} __attribute__((__packed__));
enum opahrs_msg_v1_tag {
@ -308,6 +329,7 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_REQ_GPS,
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,
@ -316,6 +338,7 @@ enum opahrs_msg_v1_tag {
OPAHRS_MSG_V1_RSP_GPS,
OPAHRS_MSG_V1_RSP_ATTITUDERAW,
OPAHRS_MSG_V1_RSP_ATTITUDE,
OPAHRS_MSG_V1_RSP_CALIBRATION,
};
struct opahrs_msg_v1_payload {

196
ground/src/plugins/uavobjects/ahrscalibration.cpp Normal file
View File

@ -0,0 +1,196 @@
/**
******************************************************************************
*
* @file ahrscalibration.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectsPlugin UAVObjects Plugin
* @{
*
* @note Object definition file: ahrscalibration.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @brief The UAVUObjects GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "ahrscalibration.h"
#include "uavobjectfield.h"
const QString AHRSCalibration::NAME = QString("AHRSCalibration");
/**
* Constructor
*/
AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
{
// Create fields
QList<UAVObjectField*> fields;
QStringList measure_varElemNames;
measure_varElemNames.append("0");
QStringList measure_varEnumOptions;
measure_varEnumOptions.append("FALSE");
measure_varEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("measure_var"), QString(""), UAVObjectField::ENUM, measure_varElemNames, measure_varEnumOptions) );
QStringList accel_biasElemNames;
accel_biasElemNames.append("X");
accel_biasElemNames.append("Y");
accel_biasElemNames.append("Z");
fields.append( new UAVObjectField(QString("accel_bias"), QString("raw"), UAVObjectField::INT16, accel_biasElemNames, QStringList()) );
QStringList accel_scaleElemNames;
accel_scaleElemNames.append("X");
accel_scaleElemNames.append("Y");
accel_scaleElemNames.append("Z");
fields.append( new UAVObjectField(QString("accel_scale"), QString("m/s"), UAVObjectField::FLOAT32, accel_scaleElemNames, QStringList()) );
QStringList accel_varElemNames;
accel_varElemNames.append("X");
accel_varElemNames.append("Y");
accel_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("accel_var"), QString("m^2/s^s"), UAVObjectField::FLOAT32, accel_varElemNames, QStringList()) );
QStringList gyro_biasElemNames;
gyro_biasElemNames.append("X");
gyro_biasElemNames.append("Y");
gyro_biasElemNames.append("Z");
fields.append( new UAVObjectField(QString("gyro_bias"), QString("raw"), UAVObjectField::INT16, gyro_biasElemNames, QStringList()) );
QStringList gyro_scaleElemNames;
gyro_scaleElemNames.append("X");
gyro_scaleElemNames.append("Y");
gyro_scaleElemNames.append("Z");
fields.append( new UAVObjectField(QString("gyro_scale"), QString("deg/s"), UAVObjectField::FLOAT32, gyro_scaleElemNames, QStringList()) );
QStringList gyro_varElemNames;
gyro_varElemNames.append("X");
gyro_varElemNames.append("Y");
gyro_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("gyro_var"), QString("deg^s/s^2"), UAVObjectField::FLOAT32, gyro_varElemNames, QStringList()) );
QStringList mag_biasElemNames;
mag_biasElemNames.append("X");
mag_biasElemNames.append("Y");
mag_biasElemNames.append("Z");
fields.append( new UAVObjectField(QString("mag_bias"), QString("mGau"), UAVObjectField::FLOAT32, mag_biasElemNames, QStringList()) );
QStringList mag_varElemNames;
mag_varElemNames.append("X");
mag_varElemNames.append("Y");
mag_varElemNames.append("Z");
fields.append( new UAVObjectField(QString("mag_var"), QString("mGau^s"), UAVObjectField::FLOAT32, mag_varElemNames, QStringList()) );
// Initialize object
initializeFields(fields, (quint8*)&data, NUMBYTES);
// Set the default field values
setDefaultFieldValues();
}
/**
* Get the default metadata for this object
*/
UAVObject::Metadata AHRSCalibration::getDefaultMetadata()
{
UAVObject::Metadata metadata;
metadata.flightAccess = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.flightTelemetryAcked = 1;
metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
metadata.flightTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER;
metadata.loggingUpdatePeriod = 0;
return metadata;
}
/**
* Initialize object fields with the default values.
* If a default value is not specified the object fields
* will be initialized to zero.
*/
void AHRSCalibration::setDefaultFieldValues()
{
data.measure_var = 1;
data.accel_bias[0] = -2048;
data.accel_bias[1] = -2048;
data.accel_bias[2] = -2048;
data.accel_scale[0] = 0.012;
data.accel_scale[1] = 0.012;
data.accel_scale[2] = 0.012;
data.accel_var[0] = 1;
data.accel_var[1] = 1;
data.accel_var[2] = 1;
data.gyro_bias[0] = -1675;
data.gyro_bias[1] = -1675;
data.gyro_bias[2] = -1675;
data.gyro_scale[0] = 0.007;
data.gyro_scale[1] = 0.007;
data.gyro_scale[2] = 0.007;
data.gyro_var[0] = 1;
data.gyro_var[1] = 1;
data.gyro_var[2] = 1;
data.mag_bias[0] = 0;
data.mag_bias[1] = 0;
data.mag_bias[2] = 0;
data.mag_var[0] = 1;
data.mag_var[1] = 1;
data.mag_var[2] = 1;
}
/**
* Get the object data fields
*/
AHRSCalibration::DataFields AHRSCalibration::getData()
{
QMutexLocker locker(mutex);
return data;
}
/**
* Set the object data fields
*/
void AHRSCalibration::setData(const DataFields& data)
{
QMutexLocker locker(mutex);
// Get metadata
Metadata mdata = getMetadata();
// Update object if the access mode permits
if ( mdata.gcsAccess == ACCESS_READWRITE )
{
this->data = data;
emit objectUpdatedAuto(this); // trigger object updated event
emit objectUpdated(this);
}
}
/**
* Create a clone of this object, a new instance ID must be specified.
* Do not use this function directly to create new instances, the
* UAVObjectManager should be used instead.
*/
UAVDataObject* AHRSCalibration::clone(quint32 instID)
{
AHRSCalibration* obj = new AHRSCalibration();
obj->initialize(instID, this->getMetaObject());
return obj;
}
/**
* Static function to retrieve an instance of the object.
*/
AHRSCalibration* AHRSCalibration::GetInstance(UAVObjectManager* objMngr, quint32 instID)
{
return dynamic_cast<AHRSCalibration*>(objMngr->getObject(AHRSCalibration::OBJID, instID));
}

128
ground/src/plugins/uavobjects/ahrscalibration.h Normal file
View File

@ -0,0 +1,128 @@
/**
******************************************************************************
*
* @file ahrscalibration.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @see The GNU Public License (GPL) Version 3
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectsPlugin UAVObjects Plugin
* @{
*
* @note Object definition file: ahrscalibration.xml.
* This is an automatically generated file.
* DO NOT modify manually.
*
* @brief The UAVUObjects GCS plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AHRSCALIBRATION_H
#define AHRSCALIBRATION_H
#include "uavdataobject.h"
#include "uavobjectmanager.h"
class UAVOBJECTS_EXPORT AHRSCalibration: public UAVDataObject
{
Q_OBJECT
public:
// Field structure
typedef struct {
quint8 measure_var;
qint16 accel_bias[3];
float accel_scale[3];
float accel_var[3];
qint16 gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
float mag_bias[3];
float mag_var[3];
} __attribute__((packed)) DataFields;
// Field information
// Field measure_var information
/* Enumeration options for field measure_var */
typedef enum { MEASURE_VAR_FALSE=0, MEASURE_VAR_TRUE=1 } measure_varOptions;
// Field accel_bias information
/* Array element names for field accel_bias */
typedef enum { ACCEL_BIAS_X=0, ACCEL_BIAS_Y=1, ACCEL_BIAS_Z=2 } accel_biasElem;
/* Number of elements for field accel_bias */
static const quint32 ACCEL_BIAS_NUMELEM = 3;
// Field accel_scale information
/* Array element names for field accel_scale */
typedef enum { ACCEL_SCALE_X=0, ACCEL_SCALE_Y=1, ACCEL_SCALE_Z=2 } accel_scaleElem;
/* Number of elements for field accel_scale */
static const quint32 ACCEL_SCALE_NUMELEM = 3;
// Field accel_var information
/* Array element names for field accel_var */
typedef enum { ACCEL_VAR_X=0, ACCEL_VAR_Y=1, ACCEL_VAR_Z=2 } accel_varElem;
/* Number of elements for field accel_var */
static const quint32 ACCEL_VAR_NUMELEM = 3;
// Field gyro_bias information
/* Array element names for field gyro_bias */
typedef enum { GYRO_BIAS_X=0, GYRO_BIAS_Y=1, GYRO_BIAS_Z=2 } gyro_biasElem;
/* Number of elements for field gyro_bias */
static const quint32 GYRO_BIAS_NUMELEM = 3;
// Field gyro_scale information
/* Array element names for field gyro_scale */
typedef enum { GYRO_SCALE_X=0, GYRO_SCALE_Y=1, GYRO_SCALE_Z=2 } gyro_scaleElem;
/* Number of elements for field gyro_scale */
static const quint32 GYRO_SCALE_NUMELEM = 3;
// Field gyro_var information
/* Array element names for field gyro_var */
typedef enum { GYRO_VAR_X=0, GYRO_VAR_Y=1, GYRO_VAR_Z=2 } gyro_varElem;
/* Number of elements for field gyro_var */
static const quint32 GYRO_VAR_NUMELEM = 3;
// Field mag_bias information
/* Array element names for field mag_bias */
typedef enum { MAG_BIAS_X=0, MAG_BIAS_Y=1, MAG_BIAS_Z=2 } mag_biasElem;
/* Number of elements for field mag_bias */
static const quint32 MAG_BIAS_NUMELEM = 3;
// Field mag_var information
/* Array element names for field mag_var */
typedef enum { MAG_VAR_X=0, MAG_VAR_Y=1, MAG_VAR_Z=2 } mag_varElem;
/* Number of elements for field mag_var */
static const quint32 MAG_VAR_NUMELEM = 3;
// Constants
static const quint32 OBJID = 3497014596U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;
static const quint32 NUMBYTES = sizeof(DataFields);
// Functions
AHRSCalibration();
DataFields getData();
void setData(const DataFields& data);
Metadata getDefaultMetadata();
UAVDataObject* clone(quint32 instID);
static AHRSCalibration* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
private:
DataFields data;
void setDefaultFieldValues();
};
#endif // AHRSCALIBRATION_H

184
ground/src/plugins/uavobjects/ahrscalibration.py Normal file
View File

@ -0,0 +1,184 @@
##
##############################################################################
#
# @file ahrscalibration.py
# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
# @brief Implementation of the AHRSCalibration object. This file has been
# automatically generated by the UAVObjectGenerator.
#
# @note Object definition file: ahrscalibration.xml.
# This is an automatically generated file.
# DO NOT modify manually.
#
# @see The GNU Public License (GPL) Version 3
#
#############################################################################/
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
# for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#
import uavobject
import struct
from collections import namedtuple
# This is a list of instances of the data fields contained in this object
_fields = [ \
uavobject.UAVObjectField(
'measure_var',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'accel_bias',
'h',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'accel_scale',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'accel_var',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'gyro_bias',
'h',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'gyro_scale',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'gyro_var',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'mag_bias',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
uavobject.UAVObjectField(
'mag_var',
'f',
3,
[
'X',
'Y',
'Z',
],
{
}
),
]
class AHRSCalibration(uavobject.UAVObject):
## Object constants
OBJID = 3497014596
NAME = "AHRSCalibration"
METANAME = "AHRSCalibrationMeta"
ISSINGLEINST = 1
ISSETTINGS = 1
def __init__(self):
uavobject.UAVObject.__init__(self,
self.OBJID,
self.NAME,
self.METANAME,
0,
self.ISSINGLEINST)
for f in _fields:
self.add_field(f)
def __str__(self):
s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n"
% (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
for f in self.get_tuple()._fields:
s += ("\t%s\n" % f)
return (s)
def main():
# Instantiate the object and dump out some interesting info
x = AHRSCalibration()
print (x)
if __name__ == "__main__":
#import pdb ; pdb.run('main()')
main()

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

@ -75,7 +75,20 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
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;
HomeSetElemNames.append("0");
QStringList HomeSetEnumOptions;
HomeSetEnumOptions.append("FALSE");
HomeSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("HomeSet"), QString(""), UAVObjectField::ENUM, HomeSetElemNames, HomeSetEnumOptions) );
QStringList CalibrationSetElemNames;
CalibrationSetElemNames.append("0");
QStringList CalibrationSetEnumOptions;
CalibrationSetEnumOptions.append("FALSE");
CalibrationSetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("CalibrationSet"), QString(""), UAVObjectField::ENUM, CalibrationSetElemNames, CalibrationSetEnumOptions) );
// Initialize object
initializeFields(fields, (quint8*)&data, NUMBYTES);

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

@ -44,7 +44,9 @@ public:
// Field structure
typedef struct {
quint8 SerialNumber[25];
quint8 CommErrors[5];
quint8 CommErrors[6];
quint8 HomeSet;
quint8 CalibrationSet;
} __attribute__((packed)) DataFields;
@ -54,13 +56,19 @@ 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 } CommErrorsElem;
typedef enum { COMMERRORS_ATTITUDE=0, COMMERRORS_ATTITUDERAW=1, COMMERRORS_POSITIONACTUAL=2, COMMERRORS_HOMELOCATION=3, COMMERRORS_ALTITUDE=4, COMMERRORS_CALIBRATION=5 } CommErrorsElem;
/* Number of elements for field CommErrors */
static const quint32 COMMERRORS_NUMELEM = 5;
static const quint32 COMMERRORS_NUMELEM = 6;
// Field HomeSet information
/* Enumeration options for field HomeSet */
typedef enum { HOMESET_FALSE=0, HOMESET_TRUE=1 } HomeSetOptions;
// Field CalibrationSet information
/* Enumeration options for field CalibrationSet */
typedef enum { CALIBRATIONSET_FALSE=0, CALIBRATIONSET_TRUE=1 } CalibrationSetOptions;
// Constants
static const quint32 OBJID = 2618706832U;
static const quint32 OBJID = 1556283776U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0;

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

@ -74,23 +74,48 @@ _fields = [ \
uavobject.UAVObjectField(
'CommErrors',
'B',
5,
6,
[
'Attitude',
'AttitudeRaw',
'PositionActual',
'HomeLocation',
'Altitude',
'Calibration',
],
{
}
),
uavobject.UAVObjectField(
'HomeSet',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'CalibrationSet',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
]
class AhrsStatus(uavobject.UAVObject):
## Object constants
OBJID = 2618706832
OBJID = 1556283776
NAME = "AhrsStatus"
METANAME = "AhrsStatusMeta"
ISSINGLEINST = 1

29
ground/src/plugins/uavobjects/homelocation.cpp
View File

@ -93,12 +93,12 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata()
UAVObject::Metadata metadata;
metadata.flightAccess = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.gcsTelemetryAcked = 0;
metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
metadata.gcsTelemetryAcked = 1;
metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.flightTelemetryAcked = 0;
metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
metadata.flightTelemetryUpdatePeriod = 10000;
metadata.flightTelemetryAcked = 1;
metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
metadata.flightTelemetryUpdatePeriod = 0;
metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER;
metadata.loggingUpdatePeriod = 0;
return metadata;
@ -111,6 +111,25 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata()
*/
void HomeLocation::setDefaultFieldValues()
{
data.Set = -1;
data.Latitude = 0;
data.Longitude = 0;
data.Altitude = 0;
data.ECEF[0] = 0;
data.ECEF[1] = 0;
data.ECEF[2] = 0;
data.RNE[0] = 0;
data.RNE[1] = 0;
data.RNE[2] = 0;
data.RNE[3] = 0;
data.RNE[4] = 0;
data.RNE[5] = 0;
data.RNE[6] = 0;
data.RNE[7] = 0;
data.RNE[8] = 0;
data.Be[0] = 0;
data.Be[1] = 0;
data.Be[2] = 0;
}

4
ground/src/plugins/uavobjects/homelocation.h
View File

@ -72,10 +72,10 @@ public:
// Constants
static const quint32 OBJID = 576787928U;
static const quint32 OBJID = 4244719376U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0;
static const bool ISSETTINGS = 1;
static const quint32 NUMBYTES = sizeof(DataFields);
// Functions

4
ground/src/plugins/uavobjects/homelocation.py
View File

@ -126,11 +126,11 @@ _fields = [ \
class HomeLocation(uavobject.UAVObject):
## Object constants
OBJID = 576787928
OBJID = 4244719376
NAME = "HomeLocation"
METANAME = "HomeLocationMeta"
ISSINGLEINST = 1
ISSETTINGS = 0
ISSETTINGS = 1
def __init__(self):
uavobject.UAVObject.__init__(self,

160
ground/src/plugins/uavobjects/uavobjects.pro
View File

@ -1,79 +1,81 @@
TEMPLATE = lib
TARGET = UAVObjects
DEFINES += UAVOBJECTS_LIBRARY
include(../../openpilotgcsplugin.pri)
include(uavobjects_dependencies.pri)
HEADERS += uavobjects_global.h \
uavobject.h \
uavmetaobject.h \
uavobjectmanager.h \
uavdataobject.h \
uavobjectfield.h \
uavobjectsinit.h \
uavobjectsplugin.h \
examplesettings.h \
ahrsstatus.h \
baroaltitude.h \
attitudeactual.h \
attitudesettings.h \
exampleobject2.h \
exampleobject1.h \
gcstelemetrystats.h \
attituderaw.h \
flighttelemetrystats.h \
systemstats.h \
systemalarms.h \
objectpersistence.h \
telemetrysettings.h \
systemsettings.h \
stabilizationsettings.h \
flightsituationactual.h \
manualcontrolsettings.h \
manualcontrolcommand.h \
attitudedesired.h \
actuatorsettings.h \
actuatordesired.h \
actuatorcommand.h \
navigationsettings.h \
navigationdesired.h \
gpsposition.h \
positionactual.h \
flightbatterystate.h \
homelocation.h
SOURCES += uavobject.cpp \
uavmetaobject.cpp \
uavobjectmanager.cpp \
uavdataobject.cpp \
uavobjectfield.cpp \
uavobjectsinit.cpp \
uavobjectsplugin.cpp \
ahrsstatus.cpp \
baroaltitude.cpp \
attitudeactual.cpp \
attitudesettings.cpp \
examplesettings.cpp \
exampleobject2.cpp \
exampleobject1.cpp \
gcstelemetrystats.cpp \
attituderaw.cpp \
flighttelemetrystats.cpp \
systemstats.cpp \
systemalarms.cpp \
objectpersistence.cpp \
telemetrysettings.cpp \
systemsettings.cpp \
stabilizationsettings.cpp \
flightsituationactual.cpp \
manualcontrolsettings.cpp \
manualcontrolcommand.cpp \
attitudedesired.cpp \
actuatorsettings.cpp \
actuatordesired.cpp \
actuatorcommand.cpp \
navigationsettings.cpp \
navigationdesired.cpp \
gpsposition.cpp \
positionactual.cpp \
flightbatterystate.cpp \
homelocation.cpp
OTHER_FILES += UAVObjects.pluginspec
TEMPLATE = lib
TARGET = UAVObjects
DEFINES += UAVOBJECTS_LIBRARY
include(../../openpilotgcsplugin.pri)
include(uavobjects_dependencies.pri)
HEADERS += uavobjects_global.h \
uavobject.h \
uavmetaobject.h \
uavobjectmanager.h \
uavdataobject.h \
uavobjectfield.h \
uavobjectsinit.h \
uavobjectsplugin.h \
examplesettings.h \
ahrsstatus.h \
ahrscalibration.h \
baroaltitude.h \
attitudeactual.h \
attitudesettings.h \
exampleobject2.h \
exampleobject1.h \
gcstelemetrystats.h \
attituderaw.h \
flighttelemetrystats.h \
systemstats.h \
systemalarms.h \
objectpersistence.h \
telemetrysettings.h \
systemsettings.h \
stabilizationsettings.h \
flightsituationactual.h \
manualcontrolsettings.h \
manualcontrolcommand.h \
attitudedesired.h \
actuatorsettings.h \
actuatordesired.h \
actuatorcommand.h \
navigationsettings.h \
navigationdesired.h \
gpsposition.h \
positionactual.h \
flightbatterystate.h \
homelocation.h
SOURCES += uavobject.cpp \
uavmetaobject.cpp \
uavobjectmanager.cpp \
uavdataobject.cpp \
uavobjectfield.cpp \
uavobjectsinit.cpp \
uavobjectsplugin.cpp \
ahrsstatus.cpp \
ahrscalibration.cpp \
baroaltitude.cpp \
attitudeactual.cpp \
attitudesettings.cpp \
examplesettings.cpp \
exampleobject2.cpp \
exampleobject1.cpp \
gcstelemetrystats.cpp \
attituderaw.cpp \
flighttelemetrystats.cpp \
systemstats.cpp \
systemalarms.cpp \
objectpersistence.cpp \
telemetrysettings.cpp \
systemsettings.cpp \
stabilizationsettings.cpp \
flightsituationactual.cpp \
manualcontrolsettings.cpp \
manualcontrolcommand.cpp \
attitudedesired.cpp \
actuatorsettings.cpp \
actuatordesired.cpp \
actuatorcommand.cpp \
navigationsettings.cpp \
navigationdesired.cpp \
gpsposition.cpp \
positionactual.cpp \
flightbatterystate.cpp \
homelocation.cpp
OTHER_FILES += UAVObjects.pluginspec

2
ground/src/plugins/uavobjects/uavobjectsinit.cpp
View File

@ -33,6 +33,7 @@
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "actuatorsettings.h"
#include "ahrscalibration.h"
#include "ahrsstatus.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
@ -70,6 +71,7 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr)
objMngr->registerObject( new ActuatorCommand() );
objMngr->registerObject( new ActuatorDesired() );
objMngr->registerObject( new ActuatorSettings() );
objMngr->registerObject( new AHRSCalibration() );
objMngr->registerObject( new AhrsStatus() );
objMngr->registerObject( new AttitudeActual() );
objMngr->registerObject( new AttitudeDesired() );

17
ground/src/shared/uavobjectdefinition/ahrscalibration.xml Normal file
View File

@ -0,0 +1,17 @@
<xml>
<object name="AHRSCalibration" singleinstance="true" settings="true">
<field name="measure_var" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="accel_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-2048"/>
<field name="accel_scale" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0.012"/>
<field name="accel_var" units="m^2/s^s" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="gyro_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-1675"/>
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0.0070"/>
<field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

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

@ -1,7 +1,9 @@
<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"/>
<field name="CommErrors" units="count" type="uint8" elementnames="Attitude,AttitudeRaw,PositionActual,HomeLocation,Altitude,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"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

20
ground/src/shared/uavobjectdefinition/homelocation.xml
View File

@ -1,15 +1,15 @@
<xml>
<object name="HomeLocation" singleinstance="true" settings="false">
<field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="Latitude" units="deg * 10e6" type="int32" elements="1"/>
<field name="Longitude" units="deg * 10e6" type="int32" elements="1"/>
<field name="Altitude" units="m over geoid" type="float" elements="1"/>
<field name="ECEF" units="m" type="float" elements="3"/>
<field name="RNE" units="" type="float" elements="9"/>
<field name="Be" units="" type="float" elements="3"/>
<object name="HomeLocation" singleinstance="true" settings="true">
<field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="false"/>
<field name="Latitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Longitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Altitude" units="m over geoid" type="float" elements="1" defaultvalue="0"/>
<field name="ECEF" units="m" type="float" elements="3" defaultvalue="0,0,0"/>
<field name="RNE" units="" type="float" elements="9" defaultvalue="0,0,0,0,0,0,0,0,0"/>
<field name="Be" units="" type="float" elements="3" defaultvalue="0,0,0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>