mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Merge branch 'next' into corvuscorax/OP-947_stateestimator-module
Conflicts: flight/modules/Attitude/revolution/attitude.c
This commit is contained in:
commit
022f1a1ac0
@ -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);
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -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,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;
|
||||
|
@ -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 */
|
||||
|
@ -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>
|
||||
|
@ -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()
|
||||
|
@ -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;
|
||||
|
Loading…
x
Reference in New Issue
Block a user