mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-16 08:29:15 +01:00
LP-73 ExtMag fix some more watchdog issues
This commit is contained in:
Cliff Geerdes
parent
d4b0d106f8
commit
b086d1a021
@ -59,6 +59,7 @@
|
||||
#include <revocalibration.h>
|
||||
#include <auxmagsettings.h>
|
||||
#include <auxmagsensor.h>
|
||||
#include <auxmagsupport.h>
|
||||
#include <accelgyrosettings.h>
|
||||
#include <revosettings.h>
|
||||
|
||||
@ -86,6 +87,8 @@
|
||||
static const TickType_t sensor_period_ticks = ((uint32_t) (1000.0f / PIOS_SENSOR_RATE / (float) portTICK_RATE_MS));
|
||||
#define AUX_MAG_SKIP ((int) ((((PIOS_SENSOR_RATE < 76) ? 76 : PIOS_SENSOR_RATE) + 74) / 75)) /* (AMS at least 2) 75 is mag ODR output data rate in pios_board.c */
|
||||
|
||||
#define AUX_MAG_LOCAL_SUPPORT
|
||||
|
||||
// Interval in number of sample to recalculate temp bias
|
||||
#define TEMP_CALIB_INTERVAL 30
|
||||
|
||||
@ -152,7 +155,9 @@ static void updateBaroTempBias(float temperature);
|
||||
static sensor_data *source_data;
|
||||
static xTaskHandle sensorsTaskHandle;
|
||||
RevoCalibrationData cal;
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
AuxMagSettingsData auxmagcal;
|
||||
#endif
|
||||
AccelGyroSettingsData agcal;
|
||||
|
||||
// These values are initialized by settings but can be updated by the attitude algorithm
|
||||
@ -160,9 +165,12 @@ static float mag_bias[3] = { 0, 0, 0 };
|
||||
static float mag_transform[3][3] = {
|
||||
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
|
||||
};
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
static float auxmag_bias[3] = { 0, 0, 0 };
|
||||
static float auxmag_transform[3][3] = {
|
||||
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
|
||||
};
|
||||
#endif
|
||||
|
||||
// Variables used to handle accel/gyro temperature bias
|
||||
static volatile bool gyro_temp_calibrated = false;
|
||||
@ -204,7 +212,9 @@ int32_t SensorsInitialize(void)
|
||||
MagSensorInitialize();
|
||||
BaroSensorInitialize();
|
||||
RevoCalibrationInitialize();
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
AuxMagSettingsInitialize();
|
||||
#endif
|
||||
RevoSettingsInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
AccelGyroSettingsInitialize();
|
||||
@ -213,7 +223,9 @@ int32_t SensorsInitialize(void)
|
||||
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
AuxMagSettingsConnectCallback(&settingsUpdatedCb);
|
||||
#endif
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
@ -485,10 +497,11 @@ static void handleMag(float *samples, float temperature)
|
||||
|
||||
static void handleAuxMag(float *samples)
|
||||
{
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
AuxMagSensorData mag;
|
||||
float mags[3] = { (float)samples[0] - mag_bias[0],
|
||||
(float)samples[1] - mag_bias[1],
|
||||
(float)samples[2] - mag_bias[2] };
|
||||
float mags[3] = { (float)samples[0] - auxmag_bias[0],
|
||||
(float)samples[1] - auxmag_bias[1],
|
||||
(float)samples[2] - auxmag_bias[2] };
|
||||
|
||||
rot_mult(auxmag_transform, mags, samples);
|
||||
|
||||
@ -498,6 +511,9 @@ static void handleAuxMag(float *samples)
|
||||
mag.Status = AUXMAGSENSOR_STATUS_OK;
|
||||
|
||||
AuxMagSensorSet(&mag);
|
||||
#else
|
||||
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
|
||||
#endif
|
||||
}
|
||||
|
||||
static void handleBaro(float sample, float temperature)
|
||||
@ -584,11 +600,16 @@ static void updateBaroTempBias(float temperature)
|
||||
static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
{
|
||||
RevoCalibrationGet(&cal);
|
||||
AuxMagSettingsGet(&auxmagcal);
|
||||
AccelGyroSettingsGet(&agcal);
|
||||
mag_bias[0] = cal.mag_bias.X;
|
||||
mag_bias[1] = cal.mag_bias.Y;
|
||||
mag_bias[2] = cal.mag_bias.Z;
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
AuxMagSettingsGet(&auxmagcal);
|
||||
auxmag_bias[0] = auxmagcal.mag_bias.X;
|
||||
auxmag_bias[1] = auxmagcal.mag_bias.Y;
|
||||
auxmag_bias[2] = auxmagcal.mag_bias.Z;
|
||||
#endif
|
||||
AccelGyroSettingsGet(&agcal);
|
||||
|
||||
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
|
||||
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
|
||||
@ -632,7 +653,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
Quaternion2R(rotationQuat, R);
|
||||
}
|
||||
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
|
||||
#ifdef AUX_MAG_LOCAL_SUPPORT
|
||||
matrix_mult_3x3f((float(*)[3])AuxMagSettingsmag_transformToArray(auxmagcal.mag_transform), R, auxmag_transform);
|
||||
#endif
|
||||
|
||||
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
|
||||
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
|
||||
|
||||
@ -114,8 +114,14 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
int32_t val = PIOS_HMC5x83_Config(dev);
|
||||
PIOS_Assert(val == 0);
|
||||
#else
|
||||
if (PIOS_HMC5x83_Config(dev) != 0) {
|
||||
return ((pios_hmc5x83_dev_t) NULL);
|
||||
}
|
||||
#endif
|
||||
|
||||
dev->data_ready = false;
|
||||
return (pios_hmc5x83_dev_t)dev;
|
||||
@ -123,7 +129,9 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
|
||||
|
||||
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler, PIOS_SENSORS_TYPE sensortype)
|
||||
{
|
||||
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
|
||||
if (handler) {
|
||||
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, sensortype, handler);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -360,7 +368,7 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
|
||||
// define this to simply return true when asking if data is available on non-GPIO devices
|
||||
// we just set the polling rate elsewhere
|
||||
// this is more efficient, but has more data time lag
|
||||
#define HMC5X83_POLLED_STATUS_RETURNS_TRUE
|
||||
#define HMC5X83_STATUS_POLL_RETURNS_TRUE
|
||||
|
||||
/**
|
||||
* @brief Tells whether new magnetometer readings are available
|
||||
@ -369,7 +377,7 @@ uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
|
||||
*/
|
||||
bool PIOS_HMC5x83_NewDataAvailable(__attribute__((unused)) pios_hmc5x83_dev_t handler)
|
||||
{
|
||||
#if ( defined(PIOS_HMC5X83_HAS_GPIOS) || !defined(HMC5X83_POLLED_STATUS_RETURNS_TRUE) )
|
||||
#if ( defined(PIOS_HMC5X83_HAS_GPIOS) || !defined(HMC5X83_STATUS_POLL_RETURNS_TRUE) )
|
||||
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
|
||||
#endif
|
||||
|
||||
@ -380,7 +388,7 @@ bool PIOS_HMC5x83_NewDataAvailable(__attribute__((unused)) pios_hmc5x83_dev_t ha
|
||||
else
|
||||
#endif /* PIOS_HMC5X83_HAS_GPIOS */
|
||||
{ // else poll to see if data is ready or just say "true" and set polling interval elsewhere
|
||||
#ifdef HMC5X83_POLLED_STATUS_RETURNS_TRUE
|
||||
#ifdef HMC5X83_STATUS_POLL_RETURNS_TRUE
|
||||
return true;
|
||||
#else
|
||||
// poll SR0 (RDY) here. 1 -> data ready.
|
||||
|
||||
@ -92,6 +92,7 @@ extern void StartModules();
|
||||
|
||||
#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn)
|
||||
|
||||
#if 0
|
||||
#define MODULE_INITIALISE_ALL \
|
||||
{ for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \
|
||||
if (fn->fn_minit) { \
|
||||
@ -99,13 +100,32 @@ extern void StartModules();
|
||||
} \
|
||||
initTaskDone = 1; \
|
||||
}
|
||||
#else
|
||||
#define MODULE_INITIALISE_ALL \
|
||||
{ for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \
|
||||
if (fn->fn_minit) { \
|
||||
(fn->fn_minit)(); } \
|
||||
} \
|
||||
initTaskDone = 1; \
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
#define MODULE_TASKCREATE_ALL \
|
||||
{ for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \
|
||||
if (fn->fn_tinit) { \
|
||||
(fn->fn_tinit)(); } \
|
||||
} \
|
||||
}
|
||||
#else
|
||||
#define MODULE_TASKCREATE_ALL \
|
||||
{ for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) { \
|
||||
if (fn->fn_tinit) { \
|
||||
(fn->fn_tinit)(); \
|
||||
PIOS_WDG_Clear(); \
|
||||
} \
|
||||
} }
|
||||
#endif
|
||||
|
||||
#endif /* USE_SIM_POSIX */
|
||||
|
||||
|
||||
@ -508,8 +508,12 @@ void PIOS_Board_Init(void)
|
||||
AuxMagSettingsTypeGet(&option);
|
||||
// if the aux mag type is FlexiPort then set it up
|
||||
if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
|
||||
// attach the 5x83 mag to the previously inited I2C2
|
||||
external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
|
||||
// add this sensor to the sensor task's list
|
||||
PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
|
||||
// mag alarm is cleared later, so use I2C
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag)?SYSTEMALARMS_ALARM_OK:SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_HMC5X83 */
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
@ -1021,202 +1025,3 @@ void PIOS_Board_Init(void)
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/*
|
||||
things to try:
|
||||
--------------
|
||||
tau uses single mode on external
|
||||
REMEMBER that to change this, we need to change the hack that always sets it continuous after each read?/write?
|
||||
static const struct pios_hmc5883_cfg pios_hmc5883_internal_cfg = {
|
||||
.exti_cfg = &pios_exti_hmc5883_internal_cfg,
|
||||
.M_ODR = PIOS_HMC5883_ODR_75,
|
||||
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
|
||||
.Gain = PIOS_HMC5883_GAIN_1_9,
|
||||
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
|
||||
.Default_Orientation = PIOS_HMC5883_TOP_90DEG,
|
||||
};
|
||||
|
||||
static const struct pios_hmc5883_cfg pios_hmc5883_external_cfg = {
|
||||
.M_ODR = PIOS_HMC5883_ODR_75,
|
||||
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
|
||||
.Gain = PIOS_HMC5883_GAIN_1_9,
|
||||
.Mode = PIOS_HMC5883_MODE_SINGLE,
|
||||
.Default_Orientation = PIOS_HMC5883_TOP_0DEG,
|
||||
};
|
||||
|
||||
tau does as many i2c_init()s as are enabled
|
||||
but only does PIOS_HMC5883_Init() for the one mag that is in use
|
||||
|
||||
try these in this order:
|
||||
------------------------
|
||||
current setup with tau fixes
|
||||
single mode
|
||||
only PIOS_HMC5883_Init() and register one mag
|
||||
increase several stacks
|
||||
sensors.c sensor task
|
||||
callback
|
||||
hardware interrupt stack
|
||||
research tau's PIOS_I2C_Init() and PIOS_HMC5883_Init()
|
||||
|
||||
RELOAD_WDG();
|
||||
|
||||
things to do:
|
||||
-------------
|
||||
make an ASSERT() for enum PIOS_HMC5X83_ORIENTATION must match AuxMagSettingsOrientationOptions
|
||||
fix i2c reset in all processors, not just F4
|
||||
move all I2C stuff to a lower priority sensor task
|
||||
|
||||
Things to do now:
|
||||
-----------------
|
||||
fix AUX_MAG_SKIP
|
||||
set AMS lower and see some duplicates
|
||||
set AMS to exactly what it needs to be for highest rate with no dups
|
||||
figure out mag orientation and set correct default and document how to determine what to use
|
||||
I think that chip upside down and facing forward is the best default.
|
||||
back up into a tar
|
||||
clean up code 90% at least
|
||||
examine git diff
|
||||
back up into a tar
|
||||
merge latest next into it
|
||||
test calibration
|
||||
test fly
|
||||
push
|
||||
announce
|
||||
code and TL fixes on all other boards
|
||||
push
|
||||
|
||||
for a test I moved PIOS_WDG_Init() to the end of PIOS_Board_Init()
|
||||
four work arounds:
|
||||
------------------
|
||||
disable ext mag with either UAVO setting or code
|
||||
disable int mag with code
|
||||
remove just registration
|
||||
PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
|
||||
disable watchdog
|
||||
either comment out #define PIOS_INCLUDE_WDG or comment out PIOS_WDG_Init();
|
||||
sometimes: pause in PIOS_Board_Init() with the debugger, then some next/step then cont, can't reproduce
|
||||
|
||||
does not fix it:
|
||||
----------------
|
||||
disable baro registration
|
||||
some RELOAD_WDG(); in sensors.c that I tried
|
||||
|
||||
neo6 gps/mag:
|
||||
-------------
|
||||
east south down is right side up
|
||||
south west down locks up the GCS, no it just reds the mag and reinits and so atti and stab are offline
|
||||
|
||||
mag facts:
|
||||
Revo front pointed north gives high positive X
|
||||
Revo right (servo pins) pointed north gives high Y
|
||||
Revo bottom pointed north gives high positive Z
|
||||
|
||||
neo6 bottom pointed north gives high
|
||||
neo6 bottom pointed north gives high
|
||||
neo6 bottom pointed north gives high
|
||||
|
||||
steps:
|
||||
send UNCHANGED
|
||||
point front north (and very importantly down 64 degrees for me)
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? X yes
|
||||
that is N channel
|
||||
|
||||
point bottom north
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? Y yes
|
||||
that is D channel
|
||||
|
||||
point right side north
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? Z yes
|
||||
that is E channel
|
||||
|
||||
Revo does X yes, Z yes, Y yes
|
||||
so to get that from this mag we need all positive
|
||||
NED
|
||||
maybe ask which axis moves Z
|
||||
|
||||
which one makes X move, that is north (south if negative)
|
||||
which one makes Y move, that is east (west if negative)
|
||||
which one makes Z move, that is down (up if negative)
|
||||
find that exact combination in some order
|
||||
|
||||
a test with upside down battery the desired orientation
|
||||
N channel Z neg is S
|
||||
D channel Y neg is U
|
||||
E channel X neg is W
|
||||
arrange in XZY order and that is WSU
|
||||
|
||||
should be WSU
|
||||
that is correct
|
||||
What must we do to get NDE XZY
|
||||
|
||||
=================================
|
||||
rephrase question
|
||||
what gives us XZY
|
||||
NED
|
||||
WSU
|
||||
|
||||
=================================
|
||||
send UNCHANGED
|
||||
point front north (and very importantly down 64 degrees for me)
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? X yes
|
||||
that is N channel
|
||||
|
||||
point bottom north
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? Y yes
|
||||
that is D channel
|
||||
|
||||
point right side north
|
||||
flip 180 degrees
|
||||
which axis moved the most? was it positive (then negative)? Z yes
|
||||
that is E channel
|
||||
|
||||
negative N is S and comes from Z
|
||||
negative D is U and comes from Y
|
||||
negative E is W and comes from X
|
||||
where did XZY come from? WSU
|
||||
|
||||
|
||||
wires forward and top side up
|
||||
Note that it is the largest difference.
|
||||
-100 to -900 is larger than 200 to -200
|
||||
-100 to -900 started more positive
|
||||
you can find a more accurate north after you know which axis is pointing that way
|
||||
make small rotations in yaw and pitch to change where it is pointing a little and watch for the strongest signal ON THAT AXIS
|
||||
send UNCHANGED
|
||||
point front north (and very importantly down 64 degrees for me)
|
||||
flip 180 degrees to point the back north
|
||||
which axis moved the most? did it start positive (then negative)? X neg
|
||||
that is N channel
|
||||
|
||||
point bottom north
|
||||
flip 180 degrees to point the top north
|
||||
which axis moved the most? did it start positive (then negative)? Y pos
|
||||
that is D channel
|
||||
|
||||
point right side north
|
||||
flip 180 degrees to point the left north
|
||||
which axis moved the most? did it start positive (then negative)? Z neg
|
||||
that is E channel
|
||||
|
||||
X-N = S
|
||||
Y+D = D
|
||||
Z-E = W
|
||||
where did XZY come from? SWD
|
||||
looks pointed south, not west after hours
|
||||
|
||||
|
||||
--- or? ---
|
||||
debug and hit continue when it traps
|
||||
|
||||
It appears that the 0x08000bcc address is the boot code where it goes after watchdog
|
||||
|
||||
|
||||
Neo6
|
||||
7-8 sats upside down, indoors
|
||||
*/
|
||||
|
||||
@ -128,7 +128,16 @@ void initTask(__attribute__((unused)) void *parameters)
|
||||
PIOS_Board_Init();
|
||||
|
||||
/* Initialize modules */
|
||||
#if 1
|
||||
MODULE_INITIALISE_ALL;
|
||||
#else
|
||||
for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) {
|
||||
if (fn->fn_minit) {
|
||||
(fn->fn_minit)();
|
||||
}
|
||||
}
|
||||
initTaskDone = 1;
|
||||
#endif
|
||||
|
||||
/* terminate this task */
|
||||
vTaskDelete(NULL);
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="Orientation" units="" type="enum" elements="1"
|
||||
options="EAST_NORTH_UP,SOUTH_EAST_UP,WEST_SOUTH_UP,NORTH_WEST_UP,EAST_SOUTH_DOWN,SOUTH_WEST_DOWN,WEST_NORTH_DOWN,NORTH_EAST_DOWN,UNCHANGED"
|
||||
defaultvalue="UNCHANGED"/>
|
||||
defaultvalue="NORTH_EAST_DOWN"/>
|
||||
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext,Flexi" defaultvalue="GPSV9"/>
|
||||
<field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user