diff options
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 38 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 4 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_local_position.h | 5 |
4 files changed, 26 insertions, 23 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 506bc1bf0..6df7c69b2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -180,7 +180,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; - float baro_offset = 0.0f; // baro offset from reference altitude, it's not the same as local_position.ref_alt if sonar available + float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once + float surface_offset = 0.0f; // ground level offset from reference altitude float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; @@ -365,7 +366,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* reset ground level on arm if sonar is invalid */ - if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { + if (armed.armed && !flag_armed) { flag_armed = armed.armed; baro_offset -= z_est[0]; z_est[0] = 0.0f; @@ -410,7 +411,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; // should be shifted by baro_offset before applying correction + baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } @@ -425,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr = flow.ground_distance_m + surface_offset + z_est[0]; sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; if (fabsf(sonar_corr) > params.sonar_err) { @@ -437,12 +438,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ - baro_offset += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset); - local_pos.ref_alt += sonar_corr; - local_pos.ref_timestamp = t; - z_est[0] += sonar_corr; - alt_avg -= sonar_corr; + surface_offset -= sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset); + alt_avg -= sonar_corr; // TODO check this sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; sonar_valid_time = t; @@ -459,7 +457,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { /* distance to surface */ - float flow_dist = -z_est[0] / att.R[2][2]; + float flow_dist = (- z_est[0] - surface_offset) / att.R[2][2]; /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -626,7 +624,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* baro offset correction if sonar available, * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ if (sonar_valid) { - baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt; + surface_offset -= sonar_corr * params.w_alt_sonar * dt; } /* accelerometer bias correction */ @@ -644,11 +642,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; } - accel_bias_corr[2] -= (baro_corr + baro_offset) * params.w_alt_baro * params.w_alt_baro; - - if (sonar_valid) { - accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; - } + accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { @@ -665,11 +659,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - if (sonar_valid) { - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); - } - - inertial_filter_correct(baro_corr + baro_offset, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); if (can_estimate_xy) { @@ -773,6 +763,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout; + if (local_pos.dist_bottom_valid) { + local_pos.dist_bottom = -z_est[0] - surface_offset; + } local_pos.timestamp = t; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202..7541910d1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1046,8 +1046,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.dist_bottom = buf.local_pos.dist_bottom; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e88da054..32dd4f714 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,8 +109,10 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; + float dist_bottom; uint8_t xy_flags; uint8_t z_flags; + uint8_t dist_flags; uint8_t landed; }; @@ -281,7 +283,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffLLffBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,DstB,XYFl,ZFl,DstFl,Land"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 427153782..807fc6c09 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,6 +77,11 @@ struct vehicle_local_position_s int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ + /* Distance to surface */ + float dist_bottom; /**< Distance to bottom surface (ground) */ + float v_dist_bottom; /**< Distance to bottom surface (ground) change rate */ + uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ + bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; /** |