diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-08 21:18:17 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-08 21:18:17 +0200 |
commit | c96de846b3b0b88859329b8a89fbc22cb8a72bed (patch) | |
tree | 6ea299e81349ab2da83688b0b5d4c87ca0f5c3dc /src | |
parent | ff92770305d015f11facc2c7b0305da7d5fbbf30 (diff) | |
download | px4-firmware-c96de846b3b0b88859329b8a89fbc22cb8a72bed.tar.gz px4-firmware-c96de846b3b0b88859329b8a89fbc22cb8a72bed.tar.bz2 px4-firmware-c96de846b3b0b88859329b8a89fbc22cb8a72bed.zip |
Added vehicle_local_position.ref_surface_timestamp to track surface level changes. Reference altitude updated continuosly when sonar available.
Diffstat (limited to 'src')
3 files changed, 55 insertions, 33 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e3bda64e5..8c3ca6671 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float ref_alt_prev = 0.0f; - hrt_abstime ref_alt_prev_t = 0; + hrt_abstime ref_surface_prev_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -345,11 +345,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_prev_t) { - if (ref_alt_prev_t != 0) { - /* reference alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z -= local_pos.ref_alt - ref_alt_prev; - } + if (local_pos.ref_surface_timestamp != ref_surface_prev_t) { + /* reference alt changed, don't follow large ground level changes in manual flight */ + mavlink_log_info(mavlink_fd, "[mpc] update z sp %.2f -> %.2f = %.2f", ref_alt_prev, local_pos.ref_alt, local_pos.ref_alt - ref_alt_prev); + local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; // TODO also correct XY setpoint } @@ -476,6 +475,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); @@ -675,7 +675,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* track local position reference even when not controlling position */ - ref_alt_prev_t = local_pos.ref_timestamp; + ref_surface_prev_t = local_pos.ref_surface_timestamp; ref_alt_prev = local_pos.ref_alt; /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ 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 55c8e1fb9..7a0a919ee 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -82,6 +82,7 @@ static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s static const uint32_t updates_counter_len = 1000000; static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz +static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -363,6 +364,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { 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) { flag_armed = armed.armed; @@ -370,7 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; alt_avg = 0.0f; local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.ref_timestamp = t; + local_pos.ref_surface_timestamp = t; } } @@ -439,8 +442,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* new ground level */ baro_alt0 += sonar_corr; mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.ref_surface_timestamp = t; z_est[0] += sonar_corr; alt_avg -= sonar_corr; sonar_corr = 0.0f; @@ -465,32 +467,49 @@ 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]; - /* convert raw flow to angular flow */ - float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; - /* flow measurements vector */ - float flow_m[3]; - flow_m[0] = -flow_ang[0] * flow_dist; - flow_m[1] = -flow_ang[1] * flow_dist; - flow_m[2] = z_est[1]; - /* velocity in NED */ - float flow_v[2] = { 0.0f, 0.0f }; - - /* project measurements vector to NED basis, skip Z component */ + /* check if flow if too large for accurate measurements */ + /* calculate estimated velocity in body frame */ + float body_v_est[2] = { 0.0f, 0.0f }; + for (int i = 0; i < 2; i++) { - for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; - } + body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; } - /* velocity correction */ - flow_corr[0] = flow_v[0] - x_est[1]; - flow_corr[1] = flow_v[1] - y_est[1]; - /* adjust correction weight */ - float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - flow_w = att.R[2][2] * flow_q_weight; - flow_valid = true; + /* use flow only if it is not too large according to estimate */ + // TODO add timeout for flow to allow flying without GPS + if (fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow) { + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; + } + } + + /* velocity correction */ + flow_corr[0] = flow_v[0] - x_est[1]; + flow_corr[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + flow_w = att.R[2][2] * flow_q_weight; + flow_valid = true; + + } else { + flow_w = 0.0f; + flow_valid = false; + } } else { flow_w = 0.0f; @@ -607,6 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* baro offset correction if sonar available */ if (sonar_valid) { baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = t; } /* accelerometer bias correction */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 427153782..a1425d695 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -73,6 +73,7 @@ struct vehicle_local_position_s bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ + uint64_t ref_surface_timestamp; /**< Time when reference surface was set */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ 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 */ |