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

AHRS: Numerous changes. Added CPU load monitoring for AHRS as well as some friendly debugging output options. Also added a new CalibrationMode setting to AHRS (Echo) to speed up sensor calibration. Cleaned up the 6 point calibration interface to make it more friendly, but I think Edouard will clobber it with something nice and sexy :).

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1455 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-29 06:38:30 +00:00 committed by peabody124
parent 2062dbe620
commit f202884ce8
16 changed files with 130 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
flight/AHRS/ahrs.c
View File

@ -62,6 +62,7 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler")))
// For debugging the raw sensors // For debugging the raw sensors
//#define DUMP_RAW //#define DUMP_RAW
//#define DUMP_FRIENDLY
/** /**
* @addtogroup AHRS_Definitions * @addtogroup AHRS_Definitions
@ -85,6 +86,8 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler")))
#define RAD_PER_DEGREE ( M_PI / 180 ) #define RAD_PER_DEGREE ( M_PI / 180 )
#define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE ) #define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE )
#define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */ #define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */
#define MAX_IDLE_COUNT 65e3
/** /**
* @} * @}
*/ */
@ -207,6 +210,8 @@ double BaseECEF[3] = {0, 0, 0};
float Rne[3][3]; float Rne[3][3];
//! Indicates the communications are requesting a calibration //! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE; uint8_t calibration_pending = FALSE;
//! Counter for tracking the idle level
static uint32_t idle_counter = 0;
/** /**
* @} * @}
@ -220,8 +225,7 @@ int main()
{ {
float gyro[3], accel[3], mag[3]; float gyro[3], accel[3], mag[3];
float vel[3] = {0,0,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. */ /* Brings up System using CMSIS functions, enables the LEDs. */
@ -299,9 +303,7 @@ int main()
while (1) { while (1) {
// Alive signal // Alive signal
PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED1);
loop_ctr ++;
if(calibration_pending) if(calibration_pending)
{ {
calibrate_sensors(); calibrate_sensors();
@ -312,7 +314,10 @@ int main()
PIOS_HMC5843_ReadMag(mag_data.raw.axis); PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Delay for valid data // Delay for valid data
while( ahrs_state != AHRS_DATA_READY ); idle_counter = 0;
while( ahrs_state != AHRS_DATA_READY )
idle_counter ++;
ahrs_state = AHRS_PROCESSING; ahrs_state = AHRS_PROCESSING;
downsample_data(); downsample_data();
@ -387,6 +392,13 @@ int main()
} }
ahrs_state = AHRS_IDLE; ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 100), (int16_t)(accel_data.filtered.y * 100), (int16_t)(accel_data.filtered.z * 100));
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 100), (int16_t)(gyro_data.filtered.y * 100), (int16_t)(gyro_data.filtered.z * 100));
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]);
PIOS_COM_SendFormattedString(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 100), (int16_t)(Nav.q[1] * 100), (int16_t)(Nav.q[2] * 100), (int16_t)(Nav.q[3] * 100));
#endif
process_spi_request(); process_spi_request();
@ -680,9 +692,9 @@ void process_spi_request(void)
lfsm_user_set_tx_v1 (&user_tx_v1); lfsm_user_set_tx_v1 (&user_tx_v1);
break; break;
case OPAHRS_MSG_V1_REQ_CALIBRATION: case OPAHRS_MSG_V1_REQ_CALIBRATION:
if(user_rx_v1.payload.user.v.req.calibration.measure_var) { if(user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_MEASURE) {
calibration_pending = TRUE; calibration_pending = TRUE;
} else { } else if (user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_SET) {
accel_var[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0]; 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[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]; accel_var[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
@ -700,20 +712,22 @@ void process_spi_request(void)
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
INSSetGyroVar(gyro_var); INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var); INSSetMagVar(mag_var);
} }
accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0]; if(user_rx_v1.payload.user.v.req.calibration.measure_var != AHRS_ECHO) {
accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1]; /* if echoing don't set anything */
accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2]; accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0]; accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1]; accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
accel_scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2]; accel_scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0]; accel_scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1]; accel_scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2]; gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
mag_bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0]; gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
mag_bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1]; gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
mag_bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[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];
}
// echo back the values used // echo back the values used
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_CALIBRATION); 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[0] = accel_var[0];
@ -793,6 +807,9 @@ void process_spi_request(void)
user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1]; 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.Vel[2] = Nav.Vel[2];
// compute the idle fraction
user_tx_v1.payload.user.v.rsp.update.load = (MAX_IDLE_COUNT - idle_counter) * 100 / MAX_IDLE_COUNT;
dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1); lfsm_user_set_tx_v1 (&user_tx_v1);
break; break;

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

@ -221,7 +221,8 @@ static void ahrscommsTask(void* parameters)
if(( result = PIOS_OPAHRS_SetGetCalibration(&req,&rsp) ) == OPAHRS_RESULT_OK ) { if(( result = PIOS_OPAHRS_SetGetCalibration(&req,&rsp) ) == OPAHRS_RESULT_OK ) {
update_calibration(&(rsp.payload.user.v.rsp.calibration)); update_calibration(&(rsp.payload.user.v.rsp.calibration));
AHRSCalibrationIsUpdatedFlag = false; AHRSCalibrationIsUpdatedFlag = false;
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE; if(rsp.payload.user.v.rsp.calibration.measure_var != AHRS_ECHO)
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
AhrsStatusSet(&data); AhrsStatusSet(&data);
} else { } else {
@ -311,8 +312,13 @@ static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration)
{ {
AHRSCalibrationData data; AHRSCalibrationData data;
AHRSCalibrationGet(&data); AHRSCalibrationGet(&data);
if(data.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET)
calibration->measure_var = data.measure_var; calibration->measure_var = AHRS_SET;
else if(data.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE)
calibration->measure_var = AHRS_MEASURE;
else
calibration->measure_var = AHRS_ECHO;
calibration->accel_bias[0] = data.accel_bias[0]; calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1]; calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2]; calibration->accel_bias[2] = data.accel_bias[2];
@ -451,6 +457,11 @@ static void process_update(struct opahrs_msg_v1_rsp_update * update)
pos.Vel[1] = update->Vel[1]; pos.Vel[1] = update->Vel[1];
pos.Vel[2] = update->Vel[2]; pos.Vel[2] = update->Vel[2];
PositionActualSet(&pos); PositionActualSet(&pos);
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = update->load;
AhrsStatusSet(&status);
} }
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw) static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw)

2
flight/OpenPilot/UAVObjects/inc/ahrscalibration.h
View File

@ -86,7 +86,7 @@ typedef struct {
// Field information // Field information
// Field measure_var information // Field measure_var information
/* Enumeration options for field measure_var */ /* Enumeration options for field measure_var */
typedef enum { AHRSCALIBRATION_MEASURE_VAR_FALSE=0, AHRSCALIBRATION_MEASURE_VAR_TRUE=1 } AHRSCalibrationmeasure_varOptions; typedef enum { AHRSCALIBRATION_MEASURE_VAR_SET=0, AHRSCALIBRATION_MEASURE_VAR_MEASURE=1, AHRSCALIBRATION_MEASURE_VAR_ECHO=2 } AHRSCalibrationmeasure_varOptions;
// Field accel_bias information // Field accel_bias information
/* Array element names for field accel_bias */ /* 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; typedef enum { AHRSCALIBRATION_ACCEL_BIAS_X=0, AHRSCALIBRATION_ACCEL_BIAS_Y=1, AHRSCALIBRATION_ACCEL_BIAS_Z=2 } AHRSCalibrationaccel_biasElem;

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

@ -41,7 +41,7 @@
#define AHRSSTATUS_H #define AHRSSTATUS_H
// Object constants // Object constants
#define AHRSSTATUS_OBJID 842145078U #define AHRSSTATUS_OBJID 3393301276U
#define AHRSSTATUS_NAME "AhrsStatus" #define AHRSSTATUS_NAME "AhrsStatus"
#define AHRSSTATUS_METANAME "AhrsStatusMeta" #define AHRSSTATUS_METANAME "AhrsStatusMeta"
#define AHRSSTATUS_ISSINGLEINST 1 #define AHRSSTATUS_ISSINGLEINST 1
@ -72,6 +72,7 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t SerialNumber[25]; uint8_t SerialNumber[25];
uint8_t CPULoad;
uint8_t CommErrors[5]; uint8_t CommErrors[5];
uint8_t AlgorithmSet; uint8_t AlgorithmSet;
uint8_t CalibrationSet; uint8_t CalibrationSet;
@ -83,6 +84,7 @@ typedef struct {
// Field SerialNumber information // Field SerialNumber information
/* Number of elements for field SerialNumber */ /* Number of elements for field SerialNumber */
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25 #define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
// Field CPULoad information
// Field CommErrors information // Field CommErrors information
/* Array element names for field CommErrors */ /* Array element names for field CommErrors */
typedef enum { AHRSSTATUS_COMMERRORS_ALGORITHM=0, AHRSSTATUS_COMMERRORS_UPDATE=1, AHRSSTATUS_COMMERRORS_ATTITUDERAW=2, AHRSSTATUS_COMMERRORS_HOMELOCATION=3, AHRSSTATUS_COMMERRORS_CALIBRATION=4 } AhrsStatusCommErrorsElem; typedef enum { AHRSSTATUS_COMMERRORS_ALGORITHM=0, AHRSSTATUS_COMMERRORS_UPDATE=1, AHRSSTATUS_COMMERRORS_ATTITUDERAW=2, AHRSSTATUS_COMMERRORS_HOMELOCATION=3, AHRSSTATUS_COMMERRORS_CALIBRATION=4 } AhrsStatusCommErrorsElem;

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

@ -211,8 +211,10 @@ struct opahrs_msg_v1_req_update {
struct opahrs_msg_v1_req_attituderaw { struct opahrs_msg_v1_req_attituderaw {
} __attribute__((__packed__)); } __attribute__((__packed__));
enum measure_mode {AHRS_SET, AHRS_MEASURE, AHRS_ECHO};
struct opahrs_msg_v1_req_calibration { struct opahrs_msg_v1_req_calibration {
uint8_t measure_var; enum measure_mode measure_var;
uint16_t accel_bias[3]; uint16_t accel_bias[3];
float accel_scale[3]; float accel_scale[3];
float accel_var[3]; float accel_var[3];
@ -293,6 +295,7 @@ struct opahrs_msg_v1_rsp_update {
} quaternion; } quaternion;
float NED[3]; float NED[3];
float Vel[3]; float Vel[3];
uint8_t load;
} __attribute__((__packed__)); } __attribute__((__packed__));
struct opahrs_msg_v1_rsp_calibration { struct opahrs_msg_v1_rsp_calibration {

2
ground/src/experimental/SerialLogger/main.cpp
View File

@ -40,7 +40,7 @@ public:
while(1) while(1)
{ {
dat = serialPort.read(10); dat = serialPort.read(100);
qDebug() << dat; qDebug() << dat;
ts << dat; ts << dat;
} }

36
ground/src/plugins/config/ahrs.ui
View File

@ -165,45 +165,13 @@ Takes about 30 seconds max.</string>
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>220</x> <x>220</x>
<y>290</y> <y>250</y>
<width>111</width> <width>111</width>
<height>27</height> <height>27</height>
</rect> </rect>
</property> </property>
<property name="text"> <property name="text">
<string>Save Position</string> <string>Six Point Cal</string>
</property>
</widget>
<widget class="QPushButton" name="ahrsComputeScaleBias">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>220</x>
<y>320</y>
<width>111</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>Compute</string>
</property>
</widget>
<widget class="QPushButton" name="ahrsCalibrationMode">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>220</x>
<y>260</y>
<width>111</width>
<height>27</height>
</rect>
</property>
<property name="text">
<string>Cal Mode</string>
</property> </property>
</widget> </widget>
<widget class="QTextBrowser" name="textBrowser"> <widget class="QTextBrowser" name="textBrowser">

82
ground/src/plugins/config/configahrswidget.cpp
View File

@ -163,7 +163,7 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
mag_z->setPos(startX,startY); mag_z->setPos(startX,startY);
mag_z->setTransform(QTransform::fromScale(1,0),true); mag_z->setTransform(QTransform::fromScale(1,0),true);
position = 0; position = -1;
// Fill the dropdown menus: // Fill the dropdown menus:
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
@ -177,8 +177,6 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
connect(m_ahrs->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM())); connect(m_ahrs->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM()));
connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD())); connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD()));
connect(m_ahrs->ahrsSavePosition, SIGNAL(clicked()), this, SLOT(savePositionData())); connect(m_ahrs->ahrsSavePosition, SIGNAL(clicked()), this, SLOT(savePositionData()));
connect(m_ahrs->ahrsComputeScaleBias, SIGNAL(clicked()), this, SLOT(computeScaleBias()));
connect(m_ahrs->ahrsCalibrationMode, SIGNAL(clicked()), this, SLOT(calibrationMode()));
connect(parent, SIGNAL(autopilotConnected()),this, SLOT(ahrsSettingsRequest())); connect(parent, SIGNAL(autopilotConnected()),this, SLOT(ahrsSettingsRequest()));
@ -204,12 +202,12 @@ void ConfigAHRSWidget::showEvent(QShowEvent *event)
*/ */
void ConfigAHRSWidget::launchAHRSCalibration() void ConfigAHRSWidget::launchAHRSCalibration()
{ {
m_ahrs->calibInstructions->setText("Calibration launched..."); m_ahrs->calibInstructions->setText("Estimating sensor variance...");
m_ahrs->ahrsCalibStart->setEnabled(false); m_ahrs->ahrsCalibStart->setEnabled(false);
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
UAVObjectField *field = obj->getField(QString("measure_var")); UAVObjectField *field = obj->getField(QString("measure_var"));
field->setValue("TRUE"); field->setValue("MEASURE");
obj->updated(); obj->updated();
QTimer::singleShot(calibrationDelay*1000, this, SLOT(calibPhase2())); QTimer::singleShot(calibrationDelay*1000, this, SLOT(calibPhase2()));
@ -245,42 +243,19 @@ void ConfigAHRSWidget::incrementProgress()
void ConfigAHRSWidget::calibPhase2() void ConfigAHRSWidget::calibPhase2()
{ {
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
UAVObjectField *field = obj->getField(QString("measure_var"));
// This is a bit weird, but it is because we are expecting an update from the // We need to echo back the results of calibration before changing to set mode
// OP board with the correct calibration values, and those only arrive on the object update m_ahrs->calibInstructions->setText("Getting results...");
// which comes back from the board, and not the first object update signal which is in fast field->setValue("ECHO");
// the object update we did ourselves... Clear ? obj->updated();
switch (phaseCounter) {
case 0:
phaseCounter++;
m_ahrs->calibInstructions->setText("Getting results...");
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2()));
obj->updated();
break;
case 1: // this is where we end up with the update just above
phaseCounter++;
break;
case 2: { // This is the update with the right values (coming from the board)
disconnect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(calibPhase2()));
// Now update size of all the graphs
drawVariancesGraph();
// Now wait 15 more seconds before re-enabling the "Save" button // Now update size of all the graphs
QTimer::singleShot(calibrationDelay*1000, this, SLOT(calibPhase2())); drawVariancesGraph();
m_ahrs->calibInstructions->setText(QString("Saving the results..."));
progressBarIndex = 0;
phaseCounter++;
}
break;
case 3: // This step saves the configuration.
saveAHRSCalibration();
m_ahrs->calibInstructions->setText(QString("Calibration saved."));
m_ahrs->ahrsCalibStart->setEnabled(true);
break;
}
saveAHRSCalibration();
m_ahrs->calibInstructions->setText(QString("Calibration saved."));
m_ahrs->ahrsCalibStart->setEnabled(true);
} }
/** /**
@ -290,7 +265,7 @@ void ConfigAHRSWidget::saveAHRSCalibration()
{ {
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
UAVObjectField *field = obj->getField(QString("measure_var")); UAVObjectField *field = obj->getField(QString("measure_var"));
field->setValue("FALSE"); field->setValue("SET");
obj->updated(); obj->updated();
updateObjectPersistance(ObjectPersistence::OPERATION_SAVE, obj); updateObjectPersistance(ObjectPersistence::OPERATION_SAVE, obj);
@ -300,7 +275,15 @@ void ConfigAHRSWidget::saveAHRSCalibration()
* Saves the data from the aircraft in one of six positions * Saves the data from the aircraft in one of six positions
*/ */
void ConfigAHRSWidget::savePositionData() void ConfigAHRSWidget::savePositionData()
{ {
if(position < 0)
{
calibrationMode();
m_ahrs->calibInstructions->setText("Place horizontally and click save position...");
position = 0;
return;
}
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
UAVObjectField *accel_field = obj->getField(QString("accels_filtered")); UAVObjectField *accel_field = obj->getField(QString("accels_filtered"));
UAVObjectField *mag_field = obj->getField(QString("magnetometers")); UAVObjectField *mag_field = obj->getField(QString("magnetometers"));
@ -313,6 +296,18 @@ void ConfigAHRSWidget::savePositionData()
mag_data_z[position] = mag_field->getValue(2).toDouble(); mag_data_z[position] = mag_field->getValue(2).toDouble();
position = (position + 1) % 6; position = (position + 1) % 6;
if(position == 1)
m_ahrs->calibInstructions->setText("Place with left side down and click save position...");
if(position == 2)
m_ahrs->calibInstructions->setText("Place upside down and click save position...");
if(position == 3)
m_ahrs->calibInstructions->setText("Place with right side down and click save position...");
if(position == 4)
m_ahrs->calibInstructions->setText("Place with nose up and click save position...");
if(position == 5)
m_ahrs->calibInstructions->setText("Place with nose down and click save position...");
if(position == 0)
computeScaleBias();
} }
//***************************************************************** //*****************************************************************
@ -442,12 +437,17 @@ void ConfigAHRSWidget::computeScaleBias()
field->setDouble(b[2] / S[2], 2); field->setDouble(b[2] / S[2], 2);
obj->updated(); obj->updated();
position = -1; //set to run again
m_ahrs->calibInstructions->setText("Computed accel and mag scale and bias...");
} }
void ConfigAHRSWidget::calibrationMode() void ConfigAHRSWidget::calibrationMode()
{ {
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
m_ahrs->calibInstructions->setText("Now place in the horizontal position...");
// set accels to unity gain // set accels to unity gain
UAVObjectField *field = obj->getField(QString("accel_scale")); UAVObjectField *field = obj->getField(QString("accel_scale"));
field->setDouble(1,0); field->setDouble(1,0);
@ -513,8 +513,6 @@ void ConfigAHRSWidget::ahrsSettingsRequest()
drawVariancesGraph(); drawVariancesGraph();
m_ahrs->ahrsCalibStart->setEnabled(true); m_ahrs->ahrsCalibStart->setEnabled(true);
m_ahrs->ahrsSavePosition->setEnabled(true); m_ahrs->ahrsSavePosition->setEnabled(true);
m_ahrs->ahrsComputeScaleBias->setEnabled(true);
m_ahrs->ahrsCalibrationMode->setEnabled(true);
m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
} }

5
ground/src/plugins/uavobjects/ahrscalibration.cpp
View File

@ -45,8 +45,9 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
QStringList measure_varElemNames; QStringList measure_varElemNames;
measure_varElemNames.append("0"); measure_varElemNames.append("0");
QStringList measure_varEnumOptions; QStringList measure_varEnumOptions;
measure_varEnumOptions.append("FALSE"); measure_varEnumOptions.append("SET");
measure_varEnumOptions.append("TRUE"); measure_varEnumOptions.append("MEASURE");
measure_varEnumOptions.append("ECHO");
fields.append( new UAVObjectField(QString("measure_var"), QString(""), UAVObjectField::ENUM, measure_varElemNames, measure_varEnumOptions) ); fields.append( new UAVObjectField(QString("measure_var"), QString(""), UAVObjectField::ENUM, measure_varElemNames, measure_varEnumOptions) );
QStringList accel_biasElemNames; QStringList accel_biasElemNames;
accel_biasElemNames.append("X"); accel_biasElemNames.append("X");

2
ground/src/plugins/uavobjects/ahrscalibration.h
View File

@ -58,7 +58,7 @@ public:
// Field information // Field information
// Field measure_var information // Field measure_var information
/* Enumeration options for field measure_var */ /* Enumeration options for field measure_var */
typedef enum { MEASURE_VAR_FALSE=0, MEASURE_VAR_TRUE=1 } measure_varOptions; typedef enum { MEASURE_VAR_SET=0, MEASURE_VAR_MEASURE=1, MEASURE_VAR_ECHO=2 } measure_varOptions;
// Field accel_bias information // Field accel_bias information
/* Array element names for field accel_bias */ /* Array element names for field accel_bias */
typedef enum { ACCEL_BIAS_X=0, ACCEL_BIAS_Y=1, ACCEL_BIAS_Z=2 } accel_biasElem; typedef enum { ACCEL_BIAS_X=0, ACCEL_BIAS_Y=1, ACCEL_BIAS_Z=2 } accel_biasElem;

5
ground/src/plugins/uavobjects/ahrscalibration.py
View File

@ -45,8 +45,9 @@ _fields = [ \
'0', '0',
], ],
{ {
'0' : 'FALSE', '0' : 'SET',
'1' : 'TRUE', '1' : 'MEASURE',
'2' : 'ECHO',
} }
), ),
uavobject.UAVObjectField( uavobject.UAVObjectField(

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

@ -69,6 +69,9 @@ AhrsStatus::AhrsStatus(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
SerialNumberElemNames.append("23"); SerialNumberElemNames.append("23");
SerialNumberElemNames.append("24"); SerialNumberElemNames.append("24");
fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("SerialNumber"), QString("n/a"), UAVObjectField::UINT8, SerialNumberElemNames, QStringList()) );
QStringList CPULoadElemNames;
CPULoadElemNames.append("0");
fields.append( new UAVObjectField(QString("CPULoad"), QString("count"), UAVObjectField::UINT8, CPULoadElemNames, QStringList()) );
QStringList CommErrorsElemNames; QStringList CommErrorsElemNames;
CommErrorsElemNames.append("Algorithm"); CommErrorsElemNames.append("Algorithm");
CommErrorsElemNames.append("Update"); CommErrorsElemNames.append("Update");

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

@ -44,6 +44,7 @@ public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 SerialNumber[25]; quint8 SerialNumber[25];
quint8 CPULoad;
quint8 CommErrors[5]; quint8 CommErrors[5];
quint8 AlgorithmSet; quint8 AlgorithmSet;
quint8 CalibrationSet; quint8 CalibrationSet;
@ -55,6 +56,7 @@ public:
// Field SerialNumber information // Field SerialNumber information
/* Number of elements for field SerialNumber */ /* Number of elements for field SerialNumber */
static const quint32 SERIALNUMBER_NUMELEM = 25; static const quint32 SERIALNUMBER_NUMELEM = 25;
// Field CPULoad information
// Field CommErrors information // Field CommErrors information
/* Array element names for field CommErrors */ /* Array element names for field CommErrors */
typedef enum { COMMERRORS_ALGORITHM=0, COMMERRORS_UPDATE=1, COMMERRORS_ATTITUDERAW=2, COMMERRORS_HOMELOCATION=3, COMMERRORS_CALIBRATION=4 } CommErrorsElem; typedef enum { COMMERRORS_ALGORITHM=0, COMMERRORS_UPDATE=1, COMMERRORS_ATTITUDERAW=2, COMMERRORS_HOMELOCATION=3, COMMERRORS_CALIBRATION=4 } CommErrorsElem;
@ -72,7 +74,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 842145078U; static const quint32 OBJID = 3393301276U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 0; static const bool ISSETTINGS = 0;

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

@ -71,6 +71,16 @@ _fields = [ \
{ {
} }
), ),
uavobject.UAVObjectField(
'CPULoad',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'CommErrors', 'CommErrors',
'B', 'B',
@ -126,7 +136,7 @@ _fields = [ \
class AhrsStatus(uavobject.UAVObject): class AhrsStatus(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 842145078 OBJID = 3393301276
NAME = "AhrsStatus" NAME = "AhrsStatus"
METANAME = "AhrsStatusMeta" METANAME = "AhrsStatusMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

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

@ -1,7 +1,7 @@
<xml> <xml>
<object name="AHRSCalibration" singleinstance="true" settings="true"> <object name="AHRSCalibration" singleinstance="true" settings="true">
<description>Contains the calibration settings for the @ref AHRSCommsModule</description> <description>Contains the calibration settings for the @ref AHRSCommsModule</description>
<field name="measure_var" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/> <field name="measure_var" units="" type="enum" elements="1" options="SET,MEASURE,ECHO" defaultvalue="SET"/>
<field name="accel_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-2048"/> <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_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="5e-5"/> <field name="accel_var" units="m^2/s^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>

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

@ -2,6 +2,7 @@
<object name="AhrsStatus" singleinstance="true" settings="false"> <object name="AhrsStatus" singleinstance="true" settings="false">
<description>Status for the @ref AHRSCommsModule, including communication errors</description> <description>Status for the @ref AHRSCommsModule, including communication errors</description>
<field name="SerialNumber" units="n/a" type="uint8" elements="25"/> <field name="SerialNumber" units="n/a" type="uint8" elements="25"/>
<field name="CPULoad" units="count" type="uint8" elements="1"/>
<field name="CommErrors" units="count" type="uint8" elementnames="Algorithm,Update,AttitudeRaw,HomeLocation,Calibration"/> <field name="CommErrors" units="count" type="uint8" elementnames="Algorithm,Update,AttitudeRaw,HomeLocation,Calibration"/>
<field name="AlgorithmSet" units="" type="enum" elements="1" options="FALSE,TRUE"/> <field name="AlgorithmSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>
<field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/> <field name="CalibrationSet" units="" type="enum" elements="1" options="FALSE,TRUE"/>