mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now.
This commit is contained in:
Anton Babushkin
parent
2be5240b9f
commit
ffc2a8b7a3
@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
memset(&control_mode, 0, sizeof(control_mode));
|
||||
|
||||
status.main_state = MAIN_STATE_MANUAL;
|
||||
status.navigation_state = NAVIGATION_STATE_STANDBY;
|
||||
status.navigation_state = NAVIGATION_STATE_DIRECT;
|
||||
status.arming_state = ARMING_STATE_INIT;
|
||||
status.hil_state = HIL_STATE_OFF;
|
||||
|
||||
@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
|
||||
}
|
||||
|
||||
/* update condition_local_position_valid */
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed);
|
||||
/* update condition_local_position_valid and condition_local_altitude_valid */
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
|
||||
|
||||
/* update battery status */
|
||||
orb_check(battery_sub, &updated);
|
||||
@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
|
||||
{
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
||||
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
|
||||
/* ARMED */
|
||||
switch (current_status->main_state) {
|
||||
case MAIN_STATE_MANUAL:
|
||||
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
|
||||
break;
|
||||
switch (current_status->main_state) {
|
||||
case MAIN_STATE_MANUAL:
|
||||
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
|
||||
break;
|
||||
|
||||
case MAIN_STATE_SEATBELT:
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
|
||||
break;
|
||||
case MAIN_STATE_SEATBELT:
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
|
||||
break;
|
||||
|
||||
case MAIN_STATE_EASY:
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
|
||||
break;
|
||||
case MAIN_STATE_EASY:
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
|
||||
break;
|
||||
|
||||
case MAIN_STATE_AUTO:
|
||||
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
/* don't act while taking off */
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
case MAIN_STATE_AUTO:
|
||||
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
|
||||
/* don't act while taking off */
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
} else {
|
||||
if (current_status->condition_landed) {
|
||||
/* if landed: transitions only to AUTO_READY are allowed */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
} else {
|
||||
if (current_status->condition_landed) {
|
||||
/* if landed: transitions only to AUTO_READY are allowed */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
} else {
|
||||
/* if not landed: */
|
||||
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
|
||||
/* act depending on switches when manual control enabled */
|
||||
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
|
||||
/* RTL */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
|
||||
|
||||
} else {
|
||||
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* MISSION */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
|
||||
|
||||
} else {
|
||||
/* LOITER */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
|
||||
}
|
||||
}
|
||||
/* if not landed: */
|
||||
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
|
||||
/* act depending on switches when manual control enabled */
|
||||
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
|
||||
/* RTL */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
|
||||
|
||||
} else {
|
||||
/* always switch to loiter in air when no RC control */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
|
||||
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
|
||||
/* MISSION */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
|
||||
|
||||
} else {
|
||||
/* LOITER */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/* always switch to loiter in air when no RC control */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* DISARMED */
|
||||
res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
|
||||
|
||||
case MAIN_STATE_SEATBELT:
|
||||
|
||||
/* need position estimate */
|
||||
// TODO only need altitude estimate really
|
||||
if (current_state->condition_local_position_valid) {
|
||||
/* need altitude estimate */
|
||||
if (current_state->condition_local_altitude_valid) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
}
|
||||
|
||||
@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
|
||||
|
||||
case MAIN_STATE_EASY:
|
||||
|
||||
/* need position estimate */
|
||||
/* need local position estimate */
|
||||
if (current_state->condition_local_position_valid) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
}
|
||||
@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
|
||||
|
||||
case MAIN_STATE_AUTO:
|
||||
|
||||
/* need position estimate */
|
||||
if (current_state->condition_local_position_valid) {
|
||||
/* need global position estimate */
|
||||
if (current_state->condition_global_position_valid) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
}
|
||||
|
||||
@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
|
||||
} else {
|
||||
|
||||
switch (new_navigation_state) {
|
||||
case NAVIGATION_STATE_STANDBY:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = false;
|
||||
control_mode->flag_control_attitude_enabled = false;
|
||||
control_mode->flag_control_velocity_enabled = false;
|
||||
control_mode->flag_control_position_enabled = false;
|
||||
control_mode->flag_control_altitude_enabled = false;
|
||||
control_mode->flag_control_climb_rate_enabled = false;
|
||||
control_mode->flag_control_manual_enabled = false;
|
||||
break;
|
||||
|
||||
case NAVIGATION_STATE_DIRECT:
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
|
||||
|
||||
case NAVIGATION_STATE_AUTO_LAND:
|
||||
|
||||
/* deny transitions from landed states */
|
||||
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
|
||||
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
|
||||
/* deny transitions from landed state */
|
||||
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
control_mode->flag_control_rates_enabled = true;
|
||||
control_mode->flag_control_attitude_enabled = true;
|
||||
|
||||
@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
||||
const float pos_ctl_dz = 0.05f;
|
||||
float home_alt = 0.0f;
|
||||
hrt_abstime home_alt_t = 0;
|
||||
uint64_t local_home_timestamp = 0;
|
||||
uint64_t local_ref_timestamp = 0;
|
||||
|
||||
PID_t xy_pos_pids[2];
|
||||
PID_t xy_vel_pids[2];
|
||||
@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
||||
} else {
|
||||
/* non-manual mode, project global setpoints to local frame */
|
||||
//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
|
||||
if (local_pos.home_timestamp != local_home_timestamp) {
|
||||
local_home_timestamp = local_pos.home_timestamp;
|
||||
if (local_pos.ref_timestamp != local_ref_timestamp) {
|
||||
local_ref_timestamp = local_pos.ref_timestamp;
|
||||
/* init local projection using local position home */
|
||||
double lat_home = local_pos.home_lat * 1e-7;
|
||||
double lon_home = local_pos.home_lon * 1e-7;
|
||||
double lat_home = local_pos.ref_lat * 1e-7;
|
||||
double lon_home = local_pos.ref_lon * 1e-7;
|
||||
map_projection_init(lat_home, lon_home);
|
||||
warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
|
||||
mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
|
||||
@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
||||
local_pos_sp.z = -global_pos_sp.altitude;
|
||||
|
||||
} else {
|
||||
local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
|
||||
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
|
||||
}
|
||||
|
||||
warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
|
||||
@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
||||
|
||||
/* manual control - move setpoints */
|
||||
if (control_mode.flag_control_manual_enabled) {
|
||||
if (local_pos.home_timestamp != home_alt_t) {
|
||||
if (local_pos.ref_timestamp != home_alt_t) {
|
||||
if (home_alt_t != 0) {
|
||||
/* home alt changed, don't follow large ground level changes in manual flight */
|
||||
local_pos_sp.z += local_pos.home_alt - home_alt;
|
||||
local_pos_sp.z += local_pos.ref_alt - home_alt;
|
||||
}
|
||||
|
||||
home_alt_t = local_pos.home_timestamp;
|
||||
home_alt = local_pos.home_alt;
|
||||
home_alt_t = local_pos.ref_timestamp;
|
||||
home_alt = local_pos.ref_alt;
|
||||
}
|
||||
|
||||
if (control_mode.flag_control_altitude_enabled) {
|
||||
|
||||
@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */
|
||||
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
|
||||
static bool verbose_mode = false;
|
||||
static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
|
||||
static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
|
||||
|
||||
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
|
||||
|
||||
@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
int baro_init_num = 200;
|
||||
float baro_alt0 = 0.0f; /* to determine while start up */
|
||||
|
||||
double lat_current = 0.0f; //[°] --> 47.0
|
||||
double lon_current = 0.0f; //[°] --> 8.5
|
||||
double alt_current = 0.0f; //[m] above MSL
|
||||
|
||||
uint32_t accel_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
|
||||
@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
/* first parameters update */
|
||||
parameters_update(&pos_inav_param_handles, ¶ms);
|
||||
|
||||
struct pollfd fds_init[2] = {
|
||||
struct pollfd fds_init[1] = {
|
||||
{ .fd = sensor_combined_sub, .events = POLLIN },
|
||||
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
|
||||
};
|
||||
|
||||
/* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */
|
||||
bool wait_gps = params.use_gps;
|
||||
/* wait for initial baro value */
|
||||
bool wait_baro = true;
|
||||
hrt_abstime wait_gps_start = 0;
|
||||
const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while ((wait_gps || wait_baro) && !thread_should_exit) {
|
||||
int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000);
|
||||
while (wait_baro && !thread_should_exit) {
|
||||
int ret = poll(fds_init, 1, 1000);
|
||||
|
||||
if (ret < 0) {
|
||||
/* poll error */
|
||||
@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
baro_alt0 /= (float) baro_init_cnt;
|
||||
warnx("init baro: alt = %.3f", baro_alt0);
|
||||
mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
|
||||
local_pos.home_alt = baro_alt0;
|
||||
local_pos.home_timestamp = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (params.use_gps && (fds_init[1].revents & POLLIN)) {
|
||||
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
|
||||
|
||||
if (wait_gps && gps.fix_type >= 3) {
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (wait_gps_start == 0) {
|
||||
wait_gps_start = t;
|
||||
|
||||
} else if (t - wait_gps_start > wait_gps_delay) {
|
||||
wait_gps = false;
|
||||
/* get GPS position for first initialization */
|
||||
lat_current = gps.lat * 1e-7;
|
||||
lon_current = gps.lon * 1e-7;
|
||||
alt_current = gps.alt * 1e-3;
|
||||
|
||||
local_pos.home_lat = lat_current * 1e7;
|
||||
local_pos.home_lon = lon_current * 1e7;
|
||||
local_pos.home_hdg = 0.0f;
|
||||
local_pos.home_timestamp = t;
|
||||
|
||||
/* initialize coordinates */
|
||||
map_projection_init(lat_current, lon_current);
|
||||
warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current);
|
||||
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current);
|
||||
local_pos.ref_alt = baro_alt0;
|
||||
local_pos.ref_timestamp = hrt_absolute_time();
|
||||
local_pos.z_valid = true;
|
||||
local_pos.v_z_valid = true;
|
||||
local_pos.global_z = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ref_xy_inited = false;
|
||||
hrt_abstime ref_xy_init_start = 0;
|
||||
const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix
|
||||
|
||||
hrt_abstime t_prev = 0;
|
||||
|
||||
uint16_t accel_updates = 0;
|
||||
@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
while (!thread_should_exit) {
|
||||
int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
|
||||
int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (ret < 0) {
|
||||
@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
baro_alt0 += sonar_corr;
|
||||
warnx("new home: alt = %.3f", baro_alt0);
|
||||
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
|
||||
local_pos.home_alt = baro_alt0;
|
||||
local_pos.home_timestamp = hrt_absolute_time();
|
||||
local_pos.ref_alt = baro_alt0;
|
||||
local_pos.ref_timestamp = hrt_absolute_time();
|
||||
z_est[0] += sonar_corr;
|
||||
sonar_corr = 0.0f;
|
||||
sonar_corr_filtered = 0.0f;
|
||||
@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
flow_updates++;
|
||||
}
|
||||
|
||||
if (params.use_gps && (fds[5].revents & POLLIN)) {
|
||||
if (fds[5].revents & POLLIN) {
|
||||
/* vehicle GPS position */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
|
||||
|
||||
if (gps.fix_type >= 3) {
|
||||
/* project GPS lat lon to plane */
|
||||
float gps_proj[2];
|
||||
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
|
||||
gps_corr[0][0] = gps_proj[0] - x_est[0];
|
||||
gps_corr[1][0] = gps_proj[1] - y_est[0];
|
||||
/* initialize reference position if needed */
|
||||
if (ref_xy_inited) {
|
||||
/* project GPS lat lon to plane */
|
||||
float gps_proj[2];
|
||||
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
|
||||
/* calculate correction for position */
|
||||
gps_corr[0][0] = gps_proj[0] - x_est[0];
|
||||
gps_corr[1][0] = gps_proj[1] - y_est[0];
|
||||
|
||||
if (gps.vel_ned_valid) {
|
||||
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
|
||||
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
|
||||
/* calculate correction for velocity */
|
||||
if (gps.vel_ned_valid) {
|
||||
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
|
||||
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
|
||||
|
||||
} else {
|
||||
gps_corr[0][1] = 0.0f;
|
||||
gps_corr[1][1] = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
gps_corr[0][1] = 0.0f;
|
||||
gps_corr[1][1] = 0.0f;
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (ref_xy_init_start == 0) {
|
||||
ref_xy_init_start = t;
|
||||
|
||||
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
|
||||
ref_xy_inited = true;
|
||||
/* reference GPS position */
|
||||
double lat = gps.lat * 1e-7;
|
||||
double lon = gps.lon * 1e-7;
|
||||
|
||||
local_pos.ref_lat = gps.lat;
|
||||
local_pos.ref_lon = gps.lon;
|
||||
local_pos.ref_timestamp = t;
|
||||
|
||||
/* initialize projection */
|
||||
map_projection_init(lat, lon);
|
||||
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
|
||||
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
|
||||
}
|
||||
}
|
||||
|
||||
gps_updates++;
|
||||
|
||||
} else {
|
||||
/* no GPS lock */
|
||||
memset(gps_corr, 0, sizeof(gps_corr));
|
||||
}
|
||||
}
|
||||
@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
|
||||
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
|
||||
|
||||
/* dont't try to estimate position when no any position source available */
|
||||
bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout;
|
||||
bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
|
||||
bool flow_valid = false; // TODO implement opt flow
|
||||
|
||||
if (can_estimate_pos) {
|
||||
/* try to estimate xy even if no absolute position source available,
|
||||
* if using optical flow velocity will be correct in this case */
|
||||
bool can_estimate_xy = gps_valid || flow_valid;
|
||||
|
||||
if (can_estimate_xy) {
|
||||
/* inertial filter prediction for position */
|
||||
inertial_filter_predict(dt, x_est);
|
||||
inertial_filter_predict(dt, y_est);
|
||||
@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
|
||||
inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
|
||||
|
||||
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
|
||||
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
|
||||
|
||||
if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) {
|
||||
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
|
||||
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
|
||||
if (gps_valid) {
|
||||
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
|
||||
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
|
||||
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
|
||||
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
|
||||
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
|
||||
if (t - pub_last > pub_interval) {
|
||||
pub_last = t;
|
||||
/* publish local position */
|
||||
local_pos.timestamp = t;
|
||||
local_pos.valid = can_estimate_pos;
|
||||
local_pos.xy_valid = can_estimate_xy && gps_valid;
|
||||
local_pos.v_xy_valid = can_estimate_xy;
|
||||
local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
|
||||
local_pos.x = x_est[0];
|
||||
local_pos.vx = x_est[1];
|
||||
local_pos.y = y_est[0];
|
||||
local_pos.vy = y_est[1];
|
||||
local_pos.z = z_est[0];
|
||||
local_pos.vz = z_est[1];
|
||||
local_pos.absolute_alt = local_pos.home_alt - local_pos.z;
|
||||
local_pos.hdg = att.yaw;
|
||||
local_pos.landed = false; // TODO
|
||||
|
||||
if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx))
|
||||
&& (isfinite(local_pos.y))
|
||||
&& (isfinite(local_pos.vy))
|
||||
&& (isfinite(local_pos.z))
|
||||
&& (isfinite(local_pos.vz))) {
|
||||
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
|
||||
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
|
||||
|
||||
if (params.use_gps) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
|
||||
|
||||
global_pos.valid = local_pos.valid;
|
||||
global_pos.timestamp = t;
|
||||
global_pos.time_gps_usec = gps.time_gps_usec;
|
||||
global_pos.lat = (int32_t)(est_lat * 1e7);
|
||||
global_pos.lon = (int32_t)(est_lon * 1e7);
|
||||
global_pos.alt = local_pos.home_alt - local_pos.z;
|
||||
global_pos.relative_alt = -local_pos.z;
|
||||
global_pos.vx = local_pos.vx;
|
||||
global_pos.vy = local_pos.vy;
|
||||
global_pos.vz = local_pos.vz;
|
||||
global_pos.hdg = local_pos.hdg;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
|
||||
}
|
||||
/* publish global position */
|
||||
global_pos.valid = local_pos.global_xy;
|
||||
if (local_pos.global_xy) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
|
||||
global_pos.lat = (int32_t)(est_lat * 1e7);
|
||||
global_pos.lon = (int32_t)(est_lon * 1e7);
|
||||
global_pos.time_gps_usec = gps.time_gps_usec;
|
||||
}
|
||||
/* set valid values even if position is not valid */
|
||||
if (local_pos.v_xy_valid) {
|
||||
global_pos.vx = local_pos.vx;
|
||||
global_pos.vy = local_pos.vy;
|
||||
global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
|
||||
}
|
||||
if (local_pos.z_valid) {
|
||||
global_pos.relative_alt = -local_pos.z;
|
||||
}
|
||||
if (local_pos.global_z) {
|
||||
global_pos.alt = local_pos.ref_alt - local_pos.z;
|
||||
}
|
||||
if (local_pos.v_z_valid) {
|
||||
global_pos.vz = local_pos.vz;
|
||||
}
|
||||
global_pos.timestamp = t;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -40,7 +40,6 @@
|
||||
|
||||
#include "position_estimator_inav_params.h"
|
||||
|
||||
PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
|
||||
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
|
||||
@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
|
||||
|
||||
int parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
{
|
||||
h->use_gps = param_find("INAV_USE_GPS");
|
||||
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
|
||||
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
|
||||
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
|
||||
@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
|
||||
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
|
||||
{
|
||||
param_get(h->use_gps, &(p->use_gps));
|
||||
param_get(h->w_alt_baro, &(p->w_alt_baro));
|
||||
param_get(h->w_alt_acc, &(p->w_alt_acc));
|
||||
param_get(h->w_alt_sonar, &(p->w_alt_sonar));
|
||||
|
||||
@ -41,7 +41,6 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
struct position_estimator_inav_params {
|
||||
int use_gps;
|
||||
float w_alt_baro;
|
||||
float w_alt_acc;
|
||||
float w_alt_sonar;
|
||||
@ -56,7 +55,6 @@ struct position_estimator_inav_params {
|
||||
};
|
||||
|
||||
struct position_estimator_inav_param_handles {
|
||||
param_t use_gps;
|
||||
param_t w_alt_baro;
|
||||
param_t w_alt_acc;
|
||||
param_t w_alt_sonar;
|
||||
|
||||
@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
|
||||
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
|
||||
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
|
||||
log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
|
||||
log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
|
||||
log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
|
||||
log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
|
||||
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;
|
||||
LOGBUFFER_WRITE_AND_COUNT(LPOS);
|
||||
}
|
||||
|
||||
|
||||
@ -106,10 +106,9 @@ struct log_LPOS_s {
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
float hdg;
|
||||
int32_t home_lat;
|
||||
int32_t home_lon;
|
||||
float home_alt;
|
||||
int32_t ref_lat;
|
||||
int32_t ref_lon;
|
||||
float ref_alt;
|
||||
};
|
||||
|
||||
/* --- LPSP - LOCAL POSITION SETPOINT --- */
|
||||
@ -260,7 +259,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, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
|
||||
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
|
||||
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"),
|
||||
|
||||
@ -54,27 +54,26 @@
|
||||
*/
|
||||
struct vehicle_local_position_s
|
||||
{
|
||||
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
|
||||
bool valid; /**< true if position satisfies validity criteria of estimator */
|
||||
|
||||
float x; /**< X positin in meters in NED earth-fixed frame */
|
||||
float y; /**< X positin in meters in NED earth-fixed frame */
|
||||
float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
|
||||
float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */
|
||||
float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */
|
||||
float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */
|
||||
float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */
|
||||
float hdg; /**< Compass heading in radians -PI..+PI. */
|
||||
|
||||
// TODO Add covariances here
|
||||
|
||||
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
|
||||
bool xy_valid; /**< true if x and y are valid */
|
||||
bool z_valid; /**< true if z is valid */
|
||||
bool v_xy_valid; /**< true if vy and vy are valid */
|
||||
bool v_z_valid; /**< true if vz is valid */
|
||||
/* Position in local NED frame */
|
||||
float x; /**< X position in meters in NED earth-fixed frame */
|
||||
float y; /**< X position in meters in NED earth-fixed frame */
|
||||
float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
|
||||
/* Velocity in NED frame */
|
||||
float vx; /**< Ground X Speed (Latitude), m/s in NED */
|
||||
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
|
||||
float vz; /**< Ground Z Speed (Altitude), m/s in NED */
|
||||
/* Reference position in GPS / WGS84 frame */
|
||||
uint64_t home_timestamp;/**< Time when home position was set */
|
||||
int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */
|
||||
int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */
|
||||
float home_alt; /**< Altitude in meters LOGME */
|
||||
float home_hdg; /**< Compass heading in radians -PI..+PI. */
|
||||
|
||||
bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
|
||||
bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */
|
||||
uint64_t ref_timestamp; /**< Time when reference position 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 */
|
||||
bool landed; /**< true if vehicle is landed */
|
||||
};
|
||||
|
||||
|
||||
@ -68,8 +68,7 @@ typedef enum {
|
||||
|
||||
/* navigation state machine */
|
||||
typedef enum {
|
||||
NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed
|
||||
NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
|
||||
NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
|
||||
NAVIGATION_STATE_STABILIZE, // attitude stabilization
|
||||
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
|
||||
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
|
||||
@ -203,6 +202,7 @@ struct vehicle_status_s
|
||||
bool condition_launch_position_valid; /**< indicates a valid launch position */
|
||||
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
|
||||
bool condition_local_position_valid;
|
||||
bool condition_local_altitude_valid;
|
||||
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
|
||||
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user