search

iNavFlight / inav

INAV: Navigation-enabled flight control software
https://inavflight.github.io
GNU General Public License v3.0
3.18k stars 1.48k forks source link

SPRacingF3 EVO compass reporting incorrect heading when tilted #590

Closed elepete closed 8 years ago

elepete commented 8 years ago

this SW is really great - experience / testing on a SP F3evo board: the GPS_HOLD mode starts after some time to drift drift is in position and altitude I have experienced this 2 times - 1:26 min and 1:06 min (rough landing not really crash)

oleost commented 8 years ago

If it continues to drift and drift my first bet are your stick are not centeres at 1500us.

If not that have a look at this post to get a hint what you should include when asking for help or submitting a bug, since without any infomation we are only guessing here.

http://www.rcgroups.com/forums/showpost.php?p=35637535&postcount=7930

This is also mentioned in the wiki https://github.com/iNavFlight/inav/wiki/1.-Getting-started-with-iNav

elepete commented 8 years ago

As the issue looked like a misaligned sensor I finally found what needs to be done mag y- or x- axis needs to be mirrored. I have not yet been flying - but watching the model on the status screen - I expect that fixes the compass issue. Is the MPU9250 - or the AK8963 potentially not correctly factory trimmed or simply inverting 1 axis?

I did that in the compass_ak8963.c file:

magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X];

//line reinserted with 1 / -1 below: magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];

if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)

//inserted line not inverted
magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
// cache mag data for reuse memcpy(cachedMagData, magData, sizeof(cachedMagData)); state = CHECK_STATUS; lastReadResult = true;

else

//inserted line -1 as original magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) \ magGain[Y]; lastReadResult = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again

endif

digitalentity commented 8 years ago

@elepete what was the result of your investigations?

elepete commented 8 years ago

Rain stopped - and I flew with mag_y = -mag_y Stable position hold in 3D poshold mode - no drift - no toilet bowl effect visible.

the initial issue was (start of issue see top), once the frame had a pitch or roll angle >20 degree the mag heading (because of the swap z-axis) changed. this caused drift / toilet bowl. as long as the frame is at level this is no issue, once roll / pitch angles go near or over magnetic inclination (in my location 64 degrees) the z-axis changes sign and the issue observed starts.

Reading through spec of AK8963 and - my preliminary conclusion is:

elepete commented 8 years ago

New compile with hard coded x1.5 multiplier on z-axis. -> mag z-axis now on sensor tab scales to ~same value as x- and y-axis -> the compass reading is now approx. a circle so i would guess this is a board issue -> send a mail to SP racing on that

hydra commented 8 years ago

@elepete It's highly doubtful there's an issue with the hardware. Perhaps @digitalentity has time to look into this a little further. I don't currently have time to investigate this further right now as I'm busy working on other issues. Maybe provide some screenshots and blackbox logs highlighting the issue.

@digitalentity is there anything you can think of that he can try? I don't have brain space right now.

hydra commented 8 years ago

@elepete If there's a specific test you'd like me someone else (me or another person) to replicate please provide a config dump, link to the exact firmware you are running and steps to repeat the test. Only this will allow someone to replicate your issue.

elepete commented 8 years ago

thank you for looking to look into that - and sorry for being late responding

I have atached - the CLI with settings INAV 1.2.1 no modifictaions - except calibration of Accelerometer and Magnetormeter (CLI dump attached) I have checked the axis with ACC and GYRO (F3 EVO board with USB on top side front, X-ax pointing fwd, Y-ax pointing left, Z-ax pointing up)

on the picture you see (calibrated sensors): left circle (inside left to right): x (blue) pointing down (~65°) to max field (mid little circle 90° mid = ~0) x pointing up (~65°) min field middle circle: y (green) the same as with x-axis right circle: z (red) the same as with x-axis while x- and y-axis have their sign in same direction, z-axis is reverse polarity.

You can also see that as at 65° angle, accelerometer above behaves the same for all 3-axis starting low (neg) and ending positive. Comparing this at x- and y-mag-axis all fine - z-mag-axis is inverted.

my conclusion is, the mag sensor has an issue. Also for calibrated mag from AKM / Invensense I am pretty sure all three axis should scale to the same end values.

Let e know if any other data is needed.

inav-sensor-2016oct07

status

System Uptime: 1698 seconds, Voltage: 0 * 0.1V (1S battery - NOT PRESENT), System load: 0.01 CPU Clock=72MHz, GYRO=MPU6500, ACC=MPU6500, BARO=BMP280, MAG=MAG_AK8963 Cycle Time: 2008, I2C Errors: 3, config size: 1996

dump

version

INAV/SPRACINGF3EVO 1.2.1 Sep 24 2016 / 10:47:22 (36954a8)

pflags

Persistent config flags: 0x00000001

dump master

mixer

mixer QUADX mmix reset smix reset

feature

feature -RX_PPM feature -VBAT feature -INFLIGHT_ACC_CAL feature -RX_SERIAL feature -MOTOR_STOP feature -SERVO_TILT feature -SOFTSERIAL feature -GPS feature -FAILSAFE feature -SONAR feature -TELEMETRY feature -CURRENT_METER feature -3D feature -RX_PARALLEL_PWM feature -RX_MSP feature -RSSI_ADC feature -LED_STRIP feature -DISPLAY feature -ONESHOT125 feature -BLACKBOX feature -CHANNEL_FORWARDING feature -TRANSPONDER feature -AIRMODE feature -SUPEREXPO feature -VTX feature -RX_NRF24 feature -SOFTSPI feature RX_PPM feature VBAT feature FAILSAFE feature TELEMETRY feature CURRENT_METER feature RSSI_ADC feature BLACKBOX feature TRANSPONDER

beeper

beeper GYRO_CALIBRATED beeper RX_LOST beeper RX_LOST_LANDING beeper DISARMING beeper ARMING beeper ARMING_GPS_FIX beeper BAT_CRIT_LOW beeper BAT_LOW beeper GPS_STATUS beeper RX_SET beeper ACC_CALIBRATION beeper ACC_CALIBRATION_FAIL beeper READY_BEEP beeper MULTI_BEEPS beeper DISARM_REPEAT beeper ARMED beeper SYSTEM_INIT beeper ON_USB

map

map AETR1234

serial

serial 20 1 115200 38400 0 115200 serial 0 0 115200 38400 0 115200 serial 1 0 115200 38400 0 115200 serial 2 0 115200 38400 0 115200

led

led 0 0,0::C:0 led 1 0,0::C:0 led 2 0,0::C:0 led 3 0,0::C:0 led 4 0,0::C:0 led 5 0,0::C:0 led 6 0,0::C:0 led 7 0,0::C:0 led 8 0,0::C:0 led 9 0,0::C:0 led 10 0,0::C:0 led 11 0,0::C:0 led 12 0,0::C:0 led 13 0,0::C:0 led 14 0,0::C:0 led 15 0,0::C:0 led 16 0,0::C:0 led 17 0,0::C:0 led 18 0,0::C:0 led 19 0,0::C:0 led 20 0,0::C:0 led 21 0,0::C:0 led 22 0,0::C:0 led 23 0,0::C:0 led 24 0,0::C:0 led 25 0,0::C:0 led 26 0,0::C:0 led 27 0,0::C:0 led 28 0,0::C:0 led 29 0,0::C:0 led 30 0,0::C:0 led 31 0,0::C:0

color

color 0 0,0,0 color 1 0,255,255 color 2 0,0,255 color 3 30,0,255 color 4 60,0,255 color 5 90,0,255 color 6 120,0,255 color 7 150,0,255 color 8 180,0,255 color 9 210,0,255 color 10 240,0,255 color 11 270,0,255 color 12 300,0,255 color 13 330,0,255 color 14 0,0,0 color 15 0,0,0

mode_color

mode_color 0 0 1 mode_color 0 1 11 mode_color 0 2 2 mode_color 0 3 13 mode_color 0 4 10 mode_color 0 5 3 mode_color 1 0 5 mode_color 1 1 11 mode_color 1 2 3 mode_color 1 3 13 mode_color 1 4 10 mode_color 1 5 3 mode_color 2 0 10 mode_color 2 1 11 mode_color 2 2 4 mode_color 2 3 13 mode_color 2 4 10 mode_color 2 5 3 mode_color 3 0 8 mode_color 3 1 11 mode_color 3 2 4 mode_color 3 3 13 mode_color 3 4 10 mode_color 3 5 3 mode_color 4 0 7 mode_color 4 1 11 mode_color 4 2 3 mode_color 4 3 13 mode_color 4 4 10 mode_color 4 5 3 mode_color 5 0 9 mode_color 5 1 11 mode_color 5 2 2 mode_color 5 3 13 mode_color 5 4 10 mode_color 5 5 3 mode_color 6 0 6 mode_color 6 1 10 mode_color 6 2 1 mode_color 6 3 0 mode_color 6 4 0 mode_color 6 5 2 mode_color 6 6 3 mode_color 6 7 6 mode_color 6 8 0 mode_color 6 9 0 mode_color 6 10 0

set looptime = 2000 set i2c_overclock = OFF set gyro_sync = OFF set gyro_sync_denom = 2 set mid_rc = 1500 set min_check = 1100 set max_check = 1900 set rssi_channel = 0 set rssi_scale = 30 set rssi_ppm_invert = OFF set rc_smoothing = ON set input_filtering_mode = OFF set min_throttle = 1150 set max_throttle = 1850 set min_command = 1000 set servo_center_pulse = 1500 set 3d_deadband_low = 1406 set 3d_deadband_high = 1514 set 3d_neutral = 1460 set 3d_deadband_throttle = 50 set motor_pwm_rate = 400 set servo_pwm_rate = 50 set disarm_kill_switch = ON set auto_disarm_delay = 5 set small_angle = 25 set reboot_character = 82 set gps_provider = UBLOX set gps_sbas_mode = NONE set gps_dyn_model = AIR_1G set gps_auto_config = ON set gps_auto_baud = ON set inav_auto_mag_decl = ON set inav_accz_unarmedcal = ON set inav_use_gps_velned = ON set inav_gps_delay = 200 set inav_gps_min_sats = 6 set inav_w_z_baro_p = 0.350 set inav_w_z_gps_p = 0.200 set inav_w_z_gps_v = 0.500 set inav_w_xy_gps_p = 1.000 set inav_w_xy_gps_v = 2.000 set inav_w_z_res_v = 0.500 set inav_w_xy_res_v = 0.500 set inav_w_acc_bias = 0.010 set inav_max_eph_epv = 1000.000 set inav_baro_epv = 100.000 set nav_disarm_on_landing = OFF set nav_use_midthr_for_althold = OFF set nav_extra_arming_safety = ON set nav_user_control_mode = ATTI set nav_position_timeout = 5 set nav_wp_radius = 100 set nav_max_speed = 300 set nav_max_climb_rate = 500 set nav_manual_speed = 500 set nav_manual_climb_rate = 200 set nav_landing_speed = 200 set nav_land_slowdown_minalt = 500 set nav_land_slowdown_maxalt = 2000 set nav_emerg_landing_speed = 500 set nav_min_rth_distance = 500 set nav_rth_tail_first = OFF set nav_rth_alt_mode = AT_LEAST set nav_rth_altitude = 1000 set nav_mc_bank_angle = 30 set nav_mc_hover_thr = 1500 set nav_mc_auto_disarm_delay = 2000 set nav_fw_cruise_thr = 1400 set nav_fw_min_thr = 1200 set nav_fw_max_thr = 1700 set nav_fw_bank_angle = 20 set nav_fw_climb_angle = 20 set nav_fw_dive_angle = 15 set nav_fw_pitch2thr = 10 set nav_fw_roll2pitch = 75 set nav_fw_loiter_radius = 5000 set serialrx_provider = SPEK1024 set spektrum_sat_bind = 0 set telemetry_switch = OFF set telemetry_inversion = ON set frsky_default_lattitude = 0.000 set frsky_default_longitude = 0.000 set frsky_coordinates_format = 0 set frsky_unit = IMPERIAL set frsky_vfas_precision = 0 set hott_alarm_sound_interval = 5 set battery_capacity = 0 set vbat_scale = 110 set vbat_max_cell_voltage = 43 set vbat_min_cell_voltage = 33 set vbat_warning_cell_voltage = 35 set current_meter_scale = 400 set current_meter_offset = 0 set multiwii_current_meter_output = OFF set current_meter_type = ADC set align_gyro = DEFAULT set align_acc = DEFAULT set align_mag = DEFAULT set align_board_roll = 0 set align_board_pitch = 0 set align_board_yaw = 0 set gyro_lpf = 42HZ set moron_threshold = 32 set imu_dcm_kp = 2500 set imu_dcm_ki = 50 set imu_dcm_kp_mag = 10000 set imu_dcm_ki_mag = 0 set pos_hold_deadband = 20 set alt_hold_deadband = 50 set yaw_motor_direction = 1 set yaw_jump_prevention_limit = 200 set tri_unarmed_servo = ON set servo_lowpass_freq = 400 set servo_lowpass_enable = OFF set failsafe_delay = 10 set failsafe_off_delay = 200 set failsafe_throttle = 1000 set failsafe_kill_switch = OFF set failsafe_throttle_low_delay = 100 set failsafe_procedure = SET-THR set rx_min_usec = 885 set rx_max_usec = 2115 set acc_hardware = 0 set baro_use_median_filter = ON set baro_hardware = 0 set mag_hardware = 0 set blackbox_rate_num = 1 set blackbox_rate_denom = 1 set blackbox_device = SERIAL set magzero_x = 497 set magzero_y = -365 set magzero_z = 537 set acczero_x = 374 set acczero_y = 122 set acczero_z = -640 set ledstrip_visual_beeper = OFF set accgain_x = 4094 set accgain_y = 4099 set accgain_z = 4037

rxfail

rxfail 0 a rxfail 1 a rxfail 2 a rxfail 3 a rxfail 4 h rxfail 5 h rxfail 6 h rxfail 7 h rxfail 8 h rxfail 9 h rxfail 10 h rxfail 11 h rxfail 12 h rxfail 13 h rxfail 14 h rxfail 15 h rxfail 16 h rxfail 17 h

dump profile

profile

profile 0

aux

aux 0 0 0 900 900 aux 1 0 0 900 900 aux 2 0 0 900 900 aux 3 0 0 900 900 aux 4 0 0 900 900 aux 5 0 0 900 900 aux 6 0 0 900 900 aux 7 0 0 900 900 aux 8 0 0 900 900 aux 9 0 0 900 900 aux 10 0 0 900 900 aux 11 0 0 900 900 aux 12 0 0 900 900 aux 13 0 0 900 900 aux 14 0 0 900 900 aux 15 0 0 900 900 aux 16 0 0 900 900 aux 17 0 0 900 900 aux 18 0 0 900 900 aux 19 0 0 900 900

adjrange

adjrange 0 0 0 900 900 0 0 adjrange 1 0 0 900 900 0 0 adjrange 2 0 0 900 900 0 0 adjrange 3 0 0 900 900 0 0 adjrange 4 0 0 900 900 0 0 adjrange 5 0 0 900 900 0 0 adjrange 6 0 0 900 900 0 0 adjrange 7 0 0 900 900 0 0 adjrange 8 0 0 900 900 0 0 adjrange 9 0 0 900 900 0 0 adjrange 10 0 0 900 900 0 0 adjrange 11 0 0 900 900 0 0

rxrange

rxrange 0 1000 2000 rxrange 1 1000 2000 rxrange 2 1000 2000 rxrange 3 1000 2000

servo

servo 0 1000 2000 1500 90 90 100 -1 servo 1 1000 2000 1500 90 90 100 -1 servo 2 1000 2000 1500 90 90 100 -1 servo 3 1000 2000 1500 90 90 100 -1 servo 4 1000 2000 1500 90 90 100 -1 servo 5 1000 2000 1500 90 90 100 -1 servo 6 1000 2000 1500 90 90 100 -1 servo 7 1000 2000 1500 90 90 100 -1

set nav_alt_p = 50 set nav_alt_i = 0 set nav_alt_d = 0 set nav_vel_p = 100 set nav_vel_i = 50 set nav_vel_d = 10 set nav_pos_p = 65 set nav_pos_i = 120 set nav_pos_d = 10 set nav_posr_p = 180 set nav_posr_i = 15 set nav_posr_d = 100 set nav_navr_p = 10 set nav_navr_i = 5 set nav_navr_d = 8 set deadband = 5 set yaw_deadband = 5 set throttle_tilt_comp_str = 0 set flaperon_throw_offset = 250 set flaperon_throw_inverted = OFF set mode_range_logic_operator = OR set default_rate_profile = 0 set gimbal_mode = NORMAL set mag_declination = 0 set mag_hold_rate_limit = 90 set p_pitch = 40 set i_pitch = 30 set d_pitch = 23 set p_roll = 40 set i_roll = 30 set d_roll = 23 set p_yaw = 85 set i_yaw = 45 set d_yaw = 0 set p_level = 20 set i_level = 15 set d_level = 75 set max_angle_inclination_rll = 300 set max_angle_inclination_pit = 300 set gyro_soft_lpf_hz = 60 set acc_soft_lpf_hz = 15 set dterm_lpf_hz = 40 set yaw_lpf_hz = 30 set yaw_p_limit = 300 set iterm_ignore_threshold = 200 set yaw_iterm_ignore_threshold = 50 set rate_accel_limit_roll_pitch = 0 set rate_accel_limit_yaw = 10000

dump rates

rateprofile

rateprofile 0

set rc_expo = 70 set rc_yaw_expo = 20 set thr_mid = 50 set thr_expo = 0 set roll_rate = 20 set pitch_rate = 20 set yaw_rate = 20 set tpa_rate = 0 set tpa_breakpoint = 1500

oleost commented 8 years ago

@elepete Hi, I'm having som issue with SPRACINGF3mini target. Spesific the magnetometer, got issue with drift and not locked in movement.

I want to try your fix. But I don't understand what you have done. You change this line only? https://github.com/iNavFlight/inav/blob/master/src/main/drivers/compass_ak8963.c#L345

And what did you replace it with?

elepete commented 8 years ago

@oleost - yes - basically you can change line https://github.com/iNavFlight/inav/blob/master/src/main/drivers/compass_ak8963.c#L345 as I needed to invert the axis I have just removed the "-" New line 345 if you want to invert mag x would look like: magData[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; in my case i just had to invert one axis.

However - I am still convinced the cleanflight / inav code is matching the datasheet of AK8963 (MPU9050). In the end in my case I did modify the https://github.com/iNavFlight/inav/blob/master/src/main/drivers/compass_ak8963.c#L242 This as I am still pretty sure the issue is with the z-axis that is also having too less output compared to x- and y-axis. magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * -45); So I have inverted with the -45 in the end of the equation; this was before a factor of 30.

I my view the question is - is the AK8963 output: a) same strength in all directions? b) are all 3 axis when pointing to north max field strength (in my case at 65° into the earth) having the same sign?

oleost commented 8 years ago

@elepete Okey, thank you. Tried it but that didnt help me.

elepete commented 8 years ago

@oleost - did you check the output of the AK8963 in the sensors tab vs. expectation and theory? i researched until I found the issue - and with "my fix" that compensates a likely wrong factory programmed or "broken" AK8963 the quad flies

oleost commented 8 years ago

@elepete This is my magnetometer when board is laying flat pointing north.

image

Laying flat and pointing west: image

elepete commented 8 years ago

@oleost - thank you: north laying flat is for my board properly calibrated: north west and laying flat: west If you are somewhere in Europe (I am in Germany) the z-axis for your and my position should have the same sign.

I do think negative z-axis is correct. If magnetic flow is direction North (and for Europe downward into the earth) plus board properly oriented: North: x-ax (pointing fwd): + magnetic flow, y-ax (pointing left): ~0, z-ax (pointing up): - magnetic flow West: x-ax (pointing left): ~0, y-ax (pointing back): - magnetic flow, z-ax (pointing up): - magnetic flow

So if my thinking is correct. You would need to align all properly that the SP F3 mini is looking like my SP F3 EVO mag sensor out.

In case all is configured correct - there may really be a systematic issue. As in my board case the z-axis is inverted... looks like it could be the same for you.

Did you check after calibration if all 3-ax have the same max. and min. values?

oleost commented 8 years ago

I live in Norway.

elepete commented 8 years ago

@oleost - did you try inverting z-axis? (sorry to be precise z-axis should be for all north of equator the same polarity)

oleost commented 8 years ago

@elepete Thanks for pointing it out, will try again tonight and report back.

digitalentity commented 8 years ago

Ok, I compared axis directions returned by current driver and compared with datasheet for MPU9250. It appears that all three axis are inverted by current driver. As there is no rotation to invert all 3 axis we have invalid vector reporting regardless of compass alignment setting.

hydra commented 8 years ago

good work everyone! I'm so glad you guys are looking at all the navigation stuff! @digitalentity @stronnag you guys rock. thanks for persevering and providing all the info and testing time @elepete and @oleost

digitalentity commented 8 years ago

@hydra more improvements (especially airplane) are coming in soon. I'm not very happy with airplane performance and that's what I intend to change next.

Community does great job finding bugs, fixing them and testing fixes. Without support from @stronnag, @oleost, @DzikuVx, @martinbudden and many many others INAV would probably never reach current level of performance.

elepete commented 8 years ago

@all Great - thank you all for looking into this - of curiosity I looked again in the datasheet of MPU9250. Theory now matches outputs. I did overlook that all 3 axis have been wrong... On my EVO board the fix #707 works as well. (sorry for being silent - was 4 days traveling for my company)

However - for me one item remains. My MPU9250 / AK8963 inside has x- and y-axis 1.5x more output than z-axis. That results in an elliptic compass reading. Resulting in some some compass drift when tilting. Is that only my AK8963? In case this is "normal" the compass calibration could or should replace the factory compensation values https://github.com/iNavFlight/inav/blob/master/src/main/drivers/compass_ak8963.c#L240-L242

digitalentity commented 8 years ago

@elepete, on my EVO all 3 axis scale equally. It's possibly flawed hardware or magnetic interference. Calibration logic can calculate scales as well, but I'm not sure it's necessary.

In worst case (in your case 90 deg tilt which is not a very realistic scenario) x1.5 error in axis scale will only result in 11 deg error which is still flyable.

I had an error of 24 deg once - forgot the sign of mag declination - and RTH still worked. Copter came home in a weird spiral path, but it still got back and landed perfectly.

digitalentity commented 8 years ago

Fix merged, closing.

maRCnh33 commented 6 years ago

As the issue looked like a misaligned sensor I finally found what needs to be done mag y- or x- axis needs to be mirrored. I have not yet been flying - but watching the model on the status screen - I expect that fixes the compass issue. Is the MPU9250 - or the AK8963 potentially not correctly factory trimmed or simply inverting 1 axis?

I did that in the compass_ak8963.c file:

magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X];

//line reinserted with 1 / -1 below: magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) magGain[Y]; magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) magGain[Z];

if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)

//inserted line not inverted magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; // cache mag data for reuse memcpy(cachedMagData, magData, sizeof(cachedMagData)); state = CHECK_STATUS; lastReadResult = true;

else

//inserted line -1 as original magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) magGain[Y]; lastReadResult = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again

endif

how can i put this data in?? i have download the inav zip file with everything in it, but how to change the compass.h file and add it to my current configuration??? https://github.com/iNavFlight/inav/issues/3987

please help me!!