Current GPS code provides altitude in meters, insufficient for good altitude hold, NMEA is capable of 10cm resolution, should use all available precission.

GPS produces LAT/LON data in degrees, NAV operates in centimeters. This complicates code and reduces code clarity. I.e. calculatePositionError takes currentPos and destinationPos in GPS points, but error is output in centimeters. Or this code:

newVelX = posError.coordinates[LAT] * posControl.pids.pos[X].param.kP;

LAT and X are all mixed up and one have to think what the heck this line does.

A good way would be to remap the beginning of coordinate system to launch point, convert sperical GPS coordinates to centimeter measurements and do flat calculations through the rest of the code. It would also simplify fusing GPS altitude, sonar altitude and baro altitude together. Adding optical flow in future would be simplified as well.

Sonar support was removed when moving to 3-PID cascade for altitude hold. Need to add it back.

Ok, seems that I'm done with major code changes. From now on most code changes will be bugfixes. A code review is now necessary to find bugs I've missed while developing this. Also latest commits intended to solve #31 introduced some major changes and I've might have broken something in process.
@dkisselev, @csurf I would appreciate your help with this 😏

Use this together with baro or instead of a baro if none available.

Tried to enable RTH today - resulted in immediate crash. Motors simply stopped. Need to investigate.

Given two conditions:

1. Flying with dead reckoning
2. Not having a valid GPS fix

We can end up getting a GPS fix after position estimator is inialised by ded-reckoning. This can (and probably will) result is a jump in estimated position and cause the machine to drift fast.

nav_rth_altitude variable is defined as UINT8, which is too small to accommodate practical altitude values in centimeters. Eg., a height value of 20m would be '2000cm', which obviously won't fit into an unsigned char. Changing the definition to UINT16 solves the problem.

Sometimes engaging poshold when altitude hold is already enabled, causes quad to gain or loose altitude

Generic i2c_nav board has pins for connecting a sonar. There should be a possibility to read from it.

1. Use GPS + Baro + Sonar together as altitude sources.
2. Keep track of relative altitude (relative to home) for NAV purposes. If craft is on the ground, unARMed, altitude for NAV purposes will always be zero and gps/baro offset it constantly adjusted.
3. Fuse Baro and GPS together to get the actual climb rate. Combine that with IMU.

When using sticks to adjust position (mostly yaw stick) copter responds, but after that it fails to maintain position and begins to fly in some direction. Disabling and re-enabling PH stops the drift.

When poshold is engaged yaw contorl does not work

Tested your latest naze hex form your dropbox today. Tried the ALThold mode on my 180 quad. Board: naze32 Full. When I put the quad in ALTHOLD it stays for a while but then starts slowly sinking to the ground. Could be the PIDs. The strange thing though is that the quad don't respond to throttle input while in althold mode. I could't raise the quad by increasing the throttle.

Poshold is possible without GPS (using dead reconing), however RTH is not. Navigation and position estimator should be made aware of possible GPS loss and handle it correctly.

Reported by @skaman82, confirmed.

I tested your 0710 build today. It would be extremely helpful to show current flightmode in mwosd like it already works in cleanflight master.

• when I activate ALTHOLD the OSD does not sho baro sensor active
• in POSHOLD we should see the "Position hold" message like in master
• in RTH we should see the "return to home" message like in master

Maybe you could integrate this functionality or simulate the master behavior.

@digitalentity I've noticed that ublox from main streamline uses 5Hz update rate, even if you set manually the gps settings to 10Hz whenever you start back the FC it will automatically reset it to 5Hz,
I've noticed that the incriminated lines are inside ..\io\gps.c, changing the line 150 with
this "0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12," will make the trick

Ideas, anyone?

One probability is porting Kalman over from https://github.com/lacker/ikalman.
However I'm blocking this until all parts of navigation-rewrite are working as they are. Improving things with Kalman may only occur once there is something to improve.

I have opened issues #1323 & #1324 in main CF code, so I found that sensors stops readings when reseting the CC3D.

I opened here because if anybody could test a CC3D board and reproduce the problems/solve them, I think here could be more people interested in fixed wings than in main CF code forums (everybody seems to play only with quadcopters :-))..

Seems like NAV PID controller code is stable enough and won't be changing much. NAV uses ALT, VEL, POS and POSR PIDs. We need to update docs with description what these PIDs do and how changed PID values will alter copter behaviour.

@digitalentity can you read sensor tab with latest nav-rewrite build, on my config the scopes do not show

Both PosR and NavR PIDs are built to achieve the same goal - convert velocity error to attitude. There is no need to have 2 PIDs as they both take the same input and provide (in theory) the same output.

Given:

1. Main goal of PH PIDs is to counteract position drift.
2. Main source of drift is wind.
3. Wind is mostly constant in speed and direction with occasionally gusts

Copter can use its velocity estimator to estimate tilt angles required to compensate wind the same way automatic throttle tilt compensation works. When wind compensation is uninitialised or not yet locked on required tilt angles common PosR PID will do it's job, but after wind compensation initialises, PosR PID will be required to only compensate for gusts, GPS drift, miscalibrated accelerometer and other minor causes of position drift. This will speed up the response or PosR PID and improve poshold stability.

Setup: use_midrc_for_althold=1, throttle input=1500, altitude drifts. Possible cause - insufficent I-term to compensate for thermals, algorithm flaw, barometer problems.

https://github.com/digitalentity/cleanflight/commit/6adef51948f198c662cb42c9c49406622529455f

the timeslice is valid for all axis.
there is no need to calculate all individual time slices.

Velocity control does not work as expected. It is possible, that problem will go away with proper NAV PIDs tuning.

I've been told that incremental PIDs are better. Description here - http://www.flightgear.org/Docs/XMLAutopilot/node3.html
This needs to be looked into.

@digitalentity I've received the i2c GPS nav-board, I've flashed with 2.2 version from EOSBAND, i2c nav board reads the data from the GPS but I can't see anything it on cleanflight configurator, is there any particular setting do I have to activate to make it work or am I using the wrong firmware?

Opened per this comment: https://github.com/digitalentity/cleanflight/issues/1#issuecomment-146441853
@bk79, can you please clarify if you activate ALTHOLD+POSHOLD or only POSHOLD mode?

@digitalentity did you have the chance to try failsafe, while I'm still at ground I was trying to force a failsafe detection but it didn't trigger, throttle was sent to 850 us but the FC sent the motor values to minimum that's all.

This is to track and discuss progress of nav-rewrite branch

@ledvinap managed to convince me that this would be a good idea 😃
This, however, will stay blocked until "Flyable" milestone is reached.

Current code calculates GPS velocity from coordinate difference. @bubi-007 suggested that raw velocity from GPS unit should be used instead. I think it's worth trying out.

I tested your 0710 build today with my 350 class quad. naze32 full (mag removed), neo-8m GPS and external compass. Average satellite count: 12. AUW 1.2 kg. Hovering point: 63% throttle.

• ALTOHOLD works like a charm. I love it!

• POSHOLD works for me ok…ish. The quad starts to fly in circles after a short while. The radius increases over time. Calibrated my compass three times but the behavior didn't change. It was little windy - 12 km/h.

• I little confused about RTH mode: Does it should keep the height by itself? When I activate RTH the quad starts immediately sinking until is hits the ground. It should come back to me and then attempt to land, right? nav_rth_alt_mode is set to 2, nav_rth_altitude is set to default 1000 (is this in cm or mm?). When I activate RTH and ALHOLD the quad is flying back in my direction but drifts to the right. It worked pretty precise in CF 1.9. I maybe have to check my compass, but I didn't explain the sinking to the ground in RTH. Tested all modes in 5-10m height and 20-40 m radius.

version

Cleanflight/NAZE 1.11.0 Oct 7 2015 / 07:05:31 (ba11933)

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 RX_PPM
feature VBAT
feature MOTOR_STOP
feature GPS
feature FAILSAFE
feature TELEMETRY
feature LED_STRIP

map

map AETR1234

serial

serial 0 1 115200 57600 0 115200
serial 1 2 115200 115200 0 115200

led

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

set looptime = 3500
set emf_avoidance = 0
set mid_rc = 1500
set min_check = 1100
set max_check = 1900
set rssi_channel = 0
set rssi_scale = 30
set rssi_ppm_invert = 0
set rc_smoothing = 1
set input_filtering_mode = 0
set min_throttle = 1050
set max_throttle = 1900
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 retarded_arm = 0
set disarm_kill_switch = 0
set auto_disarm_delay = 10
set small_angle = 25
set reboot_character = 82
set gps_provider = 1
set gps_sbas_mode = 1
set gps_auto_config = 1
set gps_auto_baud = 0
set inav_dead_reckoning = 0
set inav_gps_delay = 200
set inav_w_z_baro_p = 1.000
set inav_w_z_baro_v = 0.500
set inav_w_z_sonar_p = 2.000
set inav_w_z_sonar_v = 1.000
set inav_w_z_gps_p = 0.300
set inav_w_z_gps_v = 0.300
set inav_w_xy_gps_p = 1.000
set inav_w_xy_gps_v = 2.000
set inav_w_xy_dr_p = 0.050
set inav_w_xy_dr_v = 0.050
set inav_w_z_res_v = 0.500
set inav_w_xy_res_v = 0.500
set inav_max_eph_epv = 1000.000
set inav_sonar_epv = 20.000
set inav_baro_epv = 100.000
set nav_use_midrc_for_althold = 1
set nav_throttle_tilt_comp = 1
set nav_lock_until_takeoff = 0
set nav_user_control_mode = 0
set nav_pterm_cut_hz = 20
set nav_dterm_cut_hz = 15
set nav_wp_radius = 200
set nav_max_speed = 500
set nav_manual_speed = 500
set nav_manual_climb_rate = 200
set pos_hold_deadband = 20
set alt_hold_deadband = 40
set nav_min_rth_distance = 500
set nav_rth_alt_mode = 2
set nav_rth_altitude = 1000
set serialrx_provider = 1
set spektrum_sat_bind = 0
set telemetry_switch = 0
set telemetry_inversion = 0
set frsky_default_lattitude = 0.000
set frsky_default_longitude = 0.000
set frsky_coordinates_format = 0
set frsky_unit = 0
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 = 34
set vbat_warning_cell_voltage = 35
set current_meter_scale = 400
set current_meter_offset = 0
set multiwii_current_meter_output = 0
set current_meter_type = 1
set align_gyro = 0
set align_acc = 0
set align_mag = 2
set align_board_roll = 0
set align_board_pitch = 0
set align_board_yaw = 0
set max_angle_inclination = 350
set gyro_lpf = 42
set moron_threshold = 32
set gyro_cmpf_factor = 600
set gyro_cmpfm_factor = 250
set yaw_control_direction = 1
set pid_at_min_throttle = 1
set yaw_motor_direction = 1
set yaw_jump_prevention_limit = 200
set tri_unarmed_servo = 1
set servo_lowpass_freq = 400
set servo_lowpass_enable = 0
set failsafe_delay = 10
set failsafe_off_delay = 200
set failsafe_throttle = 1400
set failsafe_kill_switch = 0
set failsafe_throttle_low_delay = 100
set rx_min_usec = 985
set rx_max_usec = 2115
set acc_hardware = 0
set baro_hardware = 0
set mag_hardware = 0
set blackbox_rate_num = 1
set blackbox_rate_denom = 1
set blackbox_device = 1
set magzero_x = -18
set magzero_y = -281
set magzero_z = 22

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 1 0 900 2100
aux 1 23 0 1500 2100
aux 2 24 1 1350 1475
aux 3 25 1 1850 2100
aux 4 9 1 1850 2100
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 gps_pos_p = 15
set gps_pos_i = 0
set gps_pos_d = 0
set gps_posr_p = 90
set gps_posr_i = 15
set gps_posr_d = 1
set gps_nav_p = 0
set gps_nav_i = 0
set gps_nav_d = 0
set deadband = 0
set yaw_deadband = 5
set throttle_correction_value = 0
set throttle_correction_angle = 800
set default_rate_profile = 0
set gimbal_mode = 0
set acc_lpf_factor = 4
set acc_unarmedcal = 1
set acc_trim_pitch = -8
set acc_trim_roll = -6
set baro_tab_size = 7
set baro_noise_lpf = 0.350
set mag_declination = 200
set pid_controller = 3
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_pitchf = 1.600
set i_pitchf = 0.300
set d_pitchf = 0.030
set p_rollf = 1.600
set i_rollf = 0.300
set d_rollf = 0.030
set p_yawf = 5.500
set i_yawf = 0.150
set d_yawf = 0.050
set level_horizon = 3.000
set level_angle = 5.000
set sensitivity_horizon = 75
set p_alt = 30
set i_alt = 150
set d_alt = 0
set p_level = 90
set i_level = 10
set d_level = 100
set p_vel = 80
set i_vel = 50
set d_vel = 50
set yaw_p_limit = 500
set dterm_cut_hz = 0
set pterm_cut_hz = 0
set gyro_cut_hz = 0
set pid5_oldyw = 0

dump rates

rateprofile

rateprofile 0

set rc_rate = 90
set rc_expo = 50
set rc_yaw_expo = 0
set thr_mid = 55
set thr_expo = 10
set roll_rate = 0
set pitch_rate = 0
set yaw_rate = 25
set tpa_rate = 0
set tpa_breakpoint = 1500

NAV code is now very sensitive to direction of gravity and magnetic field vector in aircraft frame - for controlling accelerations and heading. Estimating attitude as fast and precise as possible is a must. Current complementary filter is doing well, but there are better algorythms available, i.e. Robert Mayhony's DCM filter or Sebastian Madgwick's IMU fusion.

During flight testing today I switched to NAV_POSHOLD mode and copter began to change pitch/roll randomly. Possible causes:

• wrong alignment or flawed calibration of magnetometer
• code error
• unhealthy default PIDs

Write unit tests to elementary navigation code.

Need to get some sane defaults for NAV_ALTHOLD PIDs - PIDALT & PIDVEL

Poshold introduces alot of jitter to the quad, automatically applied rate control is not smooth. Some sort of LPF is probably needed.

Currently IMU is sampled and updated in many places accross the NAV code. It is logical to keep all velocity-related updates in one place, that would be imu.c
NAV would only be responsible to calculating reference velocities and giving them to IMU to keep estimated velocities sane.

If NAV_ALTHOLD is enabled when craft is disarmed and on the ground, altholdInitialThrottle gets initialised to 1000. When we are trying to take off with this, actual throttle is raised slowly and that leads to a possible crash. This needs to be fixed.

I've got a ublox 6m connected on main port at 57600 bps and gps keeps going on and off even with 8 satellites anybody issuing the same problem?, my setting on ublox are 100ms the standard one set on GPS.md

Hi everybody! I'm thinking about waypoint support and I wonder how the firmware should work when there is no waypoints on the list or all waypoints were successfully visited. I have two options:

1. Hold last position
2. Execure RTH

Currently cleanflight users the number of satellites as an indicator of 'GPS health'. HDOP is a better measure of this and has been adopted by other FC platforms.

Cleanflight already has HDOP in the GPS struct (https://github.com/cleanflight/cleanflight/blob/27db9ad0fe9a4d8abc05c59db201b8944324eea4/src/main/io/gps.h#L104) but it doesn't seemed to be used or exposed.

Ticket is two things -

• expose HDOP in MSP so it is able to be shown in the cleanflight configurator
• log HDOP in blackbox logging