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

Merge branch 'next' into corvuscorax/OP-947_stateestimator-module

Browse Source 
Conflicts:
	flight/modules/Attitude/revolution/attitude.c
This commit is contained in:
Corvus Corax 2013-06-09 13:49:52 +02:00
commit 022f1a1ac0
11 changed files with 200 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
flight/modules/Attitude/revolution/attitude.c
View File

@ -325,6 +325,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
float dT;
static uint8_t init = 0;
static float gyro_bias[3] = { 0, 0, 0 };
static bool magCalibrated = true;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
@ -361,6 +362,15 @@ static int32_t updateAttitudeComplementary(bool first_run)
magData.y = 0.0f;
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
init = 0;
@ -396,21 +406,22 @@ static int32_t updateAttitudeComplementary(bool first_run)
return 0;
}
if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
if ((xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
@ -523,15 +534,15 @@ static int32_t updateAttitudeComplementary(bool first_run)
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x - gyro_bias[0]) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y - gyro_bias[1]) * rollPitchBiasRate;
gyro_bias[2] -= mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z - gyro_bias[2]) * rollPitchBiasRate;
// Correct rates based on integral coefficient
gyroStateData.x -= gyro_bias[0];
gyroStateData.y -= gyro_bias[1];
gyroStateData.z -= gyro_bias[2];
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate;
gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate;
// save gyroscope state
GyroStateSet(&gyroStateData);

2
flight/modules/GPS/GPS.c
View File

@ -117,7 +117,7 @@ int32_t GPSStart(void)
return 0;
}
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
}
return -1;
}

26
flight/modules/StateEstimation/filtercf.c
View File

@ -39,6 +39,7 @@
#include <attitudestate.h>
#include <flightstatus.h>
#include <homelocation.h>
#include <revocalibration.h>
#include <CoordinateConversions.h>
@ -62,6 +63,7 @@ struct data {
float rollPitchBiasRate;
int32_t timeval;
uint8_t init;
bool magCalibrated;
};
// Private variables
@ -87,6 +89,7 @@ static void globalInit(void)
initialized = 1;
FlightStatusInitialize();
HomeLocationInitialize();
RevoCalibrationInitialize();
FlightStatusConnectCallback(&flightStatusUpdatedCb);
flightStatusUpdatedCb(NULL);
}
@ -130,8 +133,9 @@ static int32_t maininit(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->first_run = 1;
this->accelUpdated = 0;
this->first_run = 1;
this->accelUpdated = 0;
this->magCalibrated = true;
AttitudeSettingsGet(&this->attitudeSettings);
HomeLocationGet(&this->homeLocation);
@ -221,6 +225,16 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
mag[1] = 0.0f;
mag[2] = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
this->magCalibrated = false;
mag[0] = 100.0f;
mag[1] = 0.0f;
mag[2] = 0.0f;
}
AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState);
this->init = 0;
@ -257,21 +271,21 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
return 0;
}
if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
if ((this->init == 0 && xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) {
// For first 7 seconds use accels to get gyro bias
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = 1.0f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = 1.0f;
this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f;
this->init = 0;
} else if (this->init == 0) {
// Reload settings (all the rates)
@ -371,7 +385,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;
if (this->useMag) {
this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
this->gyroBias[2] -= -mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
} else {
this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
}

31
flight/pios/inc/pios_helpers.h
View File

@ -34,7 +34,36 @@
* @return number of elements in x.
*
*/
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
/**
* @brief Compiler barrier: Disables compiler load/store reordering across the barrier
*
*/
#define COMPILER_BARRIER() asm volatile ("" ::: "memory")
// Memory barriers:
// Note that on single core Cortex M3 & M4, the is generally no need to use a processor memory barrier instruction such as DMB.
// See http://infocenter.arm.com/help/topic/com.arm.doc.dai0321a/DAI0321A_programming_guide_memory_barriers_for_m_profile.pdf
// However, it makes sense to use these if we want to reduce issues if we ever port to a multicore processor in the future.
// An important exception for STM32 is when setting up the DMA engine - see the above reference for details.
/**
* @brief Read Acquire memory barrier
*/
#define READ_MEMORY_BARRIER() COMPILER_BARRIER()
/**
* @brief Write Release memory barrier
*/
#define WRITE_MEMORY_BARRIER() COMPILER_BARRIER()
/**
* @brief Full fence memory barrier
*/
#define MEMORY_BARRIER() COMPILER_BARRIER()
// For future multicore ARM v7 or later:
// The above three macros would be replaced with: asm volatile("dmb":::"memory")
#endif // PIOS_HELPERS_H

2
flight/pios/inc/pios_usb.h
View File

@ -37,7 +37,7 @@ extern int32_t PIOS_USB_Reenumerate();
extern int32_t PIOS_USB_ChangeConnectionState(bool connected);
extern bool PIOS_USB_CableConnected(uint8_t id);
extern bool PIOS_USB_CheckAvailable(uint32_t id);
extern void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void));
#endif /* PIOS_USB_H */
/**

59
flight/pios/stm32f4xx/pios_usb.c
View File

@ -34,12 +34,14 @@
#include "usb_core.h"
#include "pios_usb_board_data.h"
#include "pios_usb_priv.h"
#include <pios_usb_priv.h>
#include <pios_helpers.h>
/* Rx/Tx status */
static uint8_t transfer_possible = 0;
static void(*disconnection_cb_list[3]) (void);
enum pios_usb_dev_magic {
PIOS_USB_DEV_MAGIC = 0x17365904,
};
@ -47,8 +49,13 @@ enum pios_usb_dev_magic {
struct pios_usb_dev {
enum pios_usb_dev_magic magic;
const struct pios_usb_cfg *cfg;
#ifdef PIOS_INCLUDE_FREERTOS
xSemaphoreHandle statusCheckSemaphore;
#endif
};
static void raiseDisconnectionCallbacks(void);
/**
* @brief Validate the usb device structure
* @returns true if valid device or false otherwise
@ -67,7 +74,8 @@ static struct pios_usb_dev *PIOS_USB_alloc(void)
if (!usb_dev) {
return NULL;
}
vSemaphoreCreateBinary(usb_dev->statusCheckSemaphore);
xSemaphoreGive(usb_dev->statusCheckSemaphore);
usb_dev->magic = PIOS_USB_DEV_MAGIC;
return usb_dev;
}
@ -162,15 +170,56 @@ uint32_t usb_found;
bool PIOS_USB_CheckAvailable(__attribute__((unused)) uint32_t id)
{
struct pios_usb_dev *usb_dev = (struct pios_usb_dev *)pios_usb_id;
static bool lastStatus = false;
if (!PIOS_USB_validate(usb_dev)) {
return false;
}
usb_found = ((usb_dev->cfg->vsense.gpio->IDR & usb_dev->cfg->vsense.init.GPIO_Pin) != 0) ^ usb_dev->cfg->vsense_active_low;
return usb_found;
return usb_found != 0 && transfer_possible ? 1 : 0;
bool status = usb_found != 0 && transfer_possible ? 1 : 0;
#ifdef PIOS_INCLUDE_FREERTOS
while (xSemaphoreTakeFromISR(usb_dev->statusCheckSemaphore, NULL) != pdTRUE) {
;
}
#endif
bool reconnect = (lastStatus && !status);
lastStatus = status;
#ifdef PIOS_INCLUDE_FREERTOS
xSemaphoreGiveFromISR(usb_dev->statusCheckSemaphore, NULL);
#endif
if (reconnect) {
raiseDisconnectionCallbacks();
}
return status;
}
/*
*
* Register a physical disconnection callback
*
*/
void PIOS_USB_RegisterDisconnectionCallback(void (*disconnectionCB)(void))
{
PIOS_Assert(disconnectionCB);
for (uint32_t i = 0; i < NELEMENTS(disconnection_cb_list); i++) {
if (disconnection_cb_list[i] == NULL) {
disconnection_cb_list[i] = disconnectionCB;
return;
}
}
PIOS_Assert(0);
}
static void raiseDisconnectionCallbacks(void)
{
uint32_t i = 0;
while (i < NELEMENTS(disconnection_cb_list) && disconnection_cb_list[i] != NULL) {
(disconnection_cb_list[i++])();
}
}
/*

29
flight/pios/stm32f4xx/pios_usb_hid.c
View File

@ -29,14 +29,14 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#include <pios.h>
#ifdef PIOS_INCLUDE_USB_HID
#include "pios_usb_hid_priv.h"
#include <pios_usb_hid_priv.h>
#include "pios_usb_board_data.h" /* PIOS_BOARD_*_DATA_LENGTH */
#include "pios_usbhook.h" /* PIOS_USBHOOK_* */
#include <pios_usbhook.h> /* PIOS_USBHOOK_* */
#include <pios_delay.h>
static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail);
@ -68,10 +68,10 @@ struct pios_usb_hid_dev {
bool usb_if_enabled;
uint8_t rx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH] __attribute__((aligned(4)));
bool rx_active;
volatile bool rx_active;
uint8_t tx_packet_buffer[PIOS_USB_BOARD_HID_DATA_LENGTH] __attribute__((aligned(4)));
bool tx_active;
volatile bool tx_active;
uint32_t rx_dropped;
uint32_t rx_oversize;
@ -187,7 +187,7 @@ static bool PIOS_USB_HID_SendReport(struct pios_usb_hid_dev *usb_hid_dev)
if (!usb_hid_dev->tx_out_cb) {
return false;
}
READ_MEMORY_BARRIER();
bool need_yield = false;
#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context,
@ -252,6 +252,12 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail)
return;
}
// add a timeout to prevent connection drops
static uint32_t last_rx_time_raw = 0;
if (PIOS_DELAY_DiffuS(last_rx_time_raw) > 1000000) {
usb_hid_dev->rx_active = false;
}
// If endpoint was stalled and there is now space make it valid
#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1;
@ -260,6 +266,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail)
#endif
if (!usb_hid_dev->rx_active && (rx_bytes_avail >= max_payload_length)) {
last_rx_time_raw = PIOS_DELAY_GetRaw();
PIOS_USBHOOK_EndpointRx(usb_hid_dev->cfg->data_rx_ep,
usb_hid_dev->rx_packet_buffer,
sizeof(usb_hid_dev->rx_packet_buffer));
@ -303,6 +310,7 @@ static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbhid_id, pios_com_callbac
* field to determine if it's ok to dereference _cb and _context
*/
usb_hid_dev->rx_in_context = context;
WRITE_MEMORY_BARRIER();
usb_hid_dev->rx_in_cb = rx_in_cb;
}
@ -319,6 +327,7 @@ static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbhid_id, pios_com_callbac
* field to determine if it's ok to dereference _cb and _context
*/
usb_hid_dev->tx_out_context = context;
WRITE_MEMORY_BARRIER();
usb_hid_dev->tx_out_cb = tx_out_cb;
}
@ -340,6 +349,8 @@ static void PIOS_USB_HID_IF_Init(uint32_t usb_hid_id)
PIOS_USB_HID_EP_OUT_Callback,
(uint32_t)usb_hid_dev);
usb_hid_dev->usb_if_enabled = true;
usb_hid_dev->tx_active = false;
usb_hid_dev->rx_active = false;
}
static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id)
@ -352,6 +363,8 @@ static void PIOS_USB_HID_IF_DeInit(uint32_t usb_hid_id)
/* DeRegister endpoint specific callbacks with the USBHOOK layer */
usb_hid_dev->usb_if_enabled = false;
usb_hid_dev->tx_active = false;
usb_hid_dev->rx_active = false;
PIOS_USBHOOK_DeRegisterEpInCallback(usb_hid_dev->cfg->data_tx_ep);
PIOS_USBHOOK_DeRegisterEpOutCallback(usb_hid_dev->cfg->data_rx_ep);
}
@ -497,7 +510,7 @@ static bool PIOS_USB_HID_EP_OUT_Callback(uint32_t usb_hid_id, __attribute__((unu
usb_hid_dev->rx_active = false;
return false;
}
READ_MEMORY_BARRIER();
/* The first byte is report ID (not checked), the second byte is the valid data length */
uint16_t headroom;
bool need_yield = false;

18
flight/pios/stm32f4xx/pios_usbhook.c
View File

@ -46,6 +46,8 @@
#include "usbd_req.h" /* USBD_CtlError */
#include "usb_dcd_int.h" /* USBD_OTG_ISR_Handler */
static void reconnect(void);
/*
* External API
*/
@ -82,6 +84,8 @@ static USBD_Usr_cb_TypeDef user_callbacks;
void PIOS_USBHOOK_Activate(void)
{
PIOS_USB_RegisterDisconnectionCallback(&reconnect);
USBD_Init(&pios_usb_otg_core_handle,
USB_OTG_FS_CORE_ID,
&device_callbacks,
@ -271,12 +275,7 @@ static USBD_DEVICE device_callbacks = {
static void PIOS_USBHOOK_USR_Init(void)
{
PIOS_USB_ChangeConnectionState(false);
#if 1
/* Force a physical disconnect/reconnect */
DCD_DevDisconnect(&pios_usb_otg_core_handle);
DCD_DevConnect(&pios_usb_otg_core_handle);
#endif
reconnect();
}
static void PIOS_USBHOOK_USR_DeviceReset(__attribute__((unused)) uint8_t speed)
@ -483,4 +482,11 @@ static USBD_Class_cb_TypeDef class_callbacks = {
#endif /* USB_SUPPORT_USER_STRING_DESC */
};
static void reconnect(void)
{
/* Force a physical disconnect/reconnect */
DCD_DevDisconnect(&pios_usb_otg_core_handle);
DCD_DevConnect(&pios_usb_otg_core_handle);
}
#endif /* PIOS_INCLUDE_USB */

16
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -1275,10 +1275,10 @@ border-radius: 5;</string>
<number>10</number>
</property>
<property name="maximum">
<number>255</number>
<number>180</number>
</property>
<property name="value">
<number>115</number>
<number>83</number>
</property>
<property name="objrelation" stdset="0">
<stringlist>
@ -2534,13 +2534,13 @@ border-radius: 5;</string>
<number>10</number>
</property>
<property name="maximum">
<number>255</number>
<number>180</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>115</number>
<number>83</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -8801,7 +8801,7 @@ border-radius: 5;</string>
<property name="geometry">
<rect>
<x>0</x>
<y>-49</y>
<y>0</y>
<width>936</width>
<height>702</height>
</rect>
@ -9907,7 +9907,7 @@ border-radius: 5;</string>
<number>0</number>
</property>
<property name="maximum">
<double>255.000000000000000</double>
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
@ -10112,7 +10112,7 @@ border-radius: 5;</string>
<number>0</number>
</property>
<property name="maximum">
<double>255.000000000000000</double>
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
@ -12049,7 +12049,7 @@ border-radius: 5;</string>
<number>0</number>
</property>
<property name="maximum">
<double>255.000000000000000</double>
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>

39
ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp
View File

@ -32,6 +32,7 @@
#include "accelstate.h"
#include "gyrostate.h"
#include "qdebug.h"
#include "revocalibration.h"
BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
@ -133,15 +134,37 @@ void BiasCalibrationUtil::startMeasurement()
m_gyroY = 0;
m_gyroZ = 0;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager);
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
Q_ASSERT(revolutionCalibration);
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = 0;
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
int i;
for (i = 0; i < 5; i++) {
RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData);
}
// Disable gyro bias correction to see raw data
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_X] = 0;
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Y] = 0;
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Z] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_X] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Y] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Z] = 0;
for (i = 0; i < 5; i++) {
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
}
// Set up to receive updates for accels
UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
@ -151,8 +174,10 @@ void BiasCalibrationUtil::startMeasurement()
UAVObject::Metadata newMetaData = m_previousAccelMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
uavObject->setMetadata(newMetaData);
for (i = 0; i < 5; i++) {
uavObject->setMetadata(newMetaData);
}
// Set up to receive updates from gyros
uavObject = GyroState::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
@ -162,7 +187,9 @@ void BiasCalibrationUtil::startMeasurement()
newMetaData = m_previousGyroMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
uavObject->setMetadata(newMetaData);
for (i = 0; i < 5; i++) {
uavObject->setMetadata(newMetaData);
}
}
void BiasCalibrationUtil::stopMeasurement()

2
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -357,6 +357,8 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE);
data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE);
data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
copterControlCalibration->setData(data);
addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings"));
break;