fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 12:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now.

Browse Source
This commit is contained in:
Anton Babushkin
2013-08-18 23:05:26 +02:00
parent 2be5240b9f
commit ffc2a8b7a3
10 changed files with 194 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+47 -53
View File
@@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&control_mode, 0, sizeof(control_mode)); memset(&control_mode, 0, sizeof(control_mode));
status.main_state = MAIN_STATE_MANUAL; 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.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF; 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); orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
} }
/* update condition_local_position_valid */ /* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); 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 */ /* update battery status */
orb_check(battery_sub, &updated); 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; transition_result_t res = TRANSITION_DENIED;
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { switch (current_status->main_state) {
/* ARMED */ case MAIN_STATE_MANUAL:
switch (current_status->main_state) { res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
case MAIN_STATE_MANUAL: break;
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT: case MAIN_STATE_SEATBELT:
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
break; break;
case MAIN_STATE_EASY: case MAIN_STATE_EASY:
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
break; break;
case MAIN_STATE_AUTO: case MAIN_STATE_AUTO:
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */ /* don't act while taking off */
res = TRANSITION_NOT_CHANGED; 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 { } else {
if (current_status->condition_landed) { /* if not landed: */
/* if landed: transitions only to AUTO_READY are allowed */ if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); /* act depending on switches when manual control enabled */
// TRANSITION_DENIED is not possible here if (current_status->return_switch == RETURN_SWITCH_RETURN) {
break; /* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} 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);
}
}
} else { } else {
/* always switch to loiter in air when no RC control */ if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); /* 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 { break;
/* DISARMED */
res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); default:
break;
} }
return res; return res;
src/modules/commander/state_machine_helper.cpp
+7 -20
View File
@@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT: case MAIN_STATE_SEATBELT:
/* need position estimate */ /* need altitude estimate */
// TODO only need altitude estimate really if (current_state->condition_local_altitude_valid) {
if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED; 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: case MAIN_STATE_EASY:
/* need position estimate */ /* need local position estimate */
if (current_state->condition_local_position_valid) { if (current_state->condition_local_position_valid) {
ret = TRANSITION_CHANGED; 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: case MAIN_STATE_AUTO:
/* need position estimate */ /* need global position estimate */
if (current_state->condition_local_position_valid) { if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
} else { } else {
switch (new_navigation_state) { 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: case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true; 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: case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed states */ /* deny transitions from landed state */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true; control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_attitude_enabled = true;
src/modules/multirotor_pos_control/multirotor_pos_control.c
+10 -10
View File
@@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
const float pos_ctl_dz = 0.05f; const float pos_ctl_dz = 0.05f;
float home_alt = 0.0f; float home_alt = 0.0f;
hrt_abstime home_alt_t = 0; 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_pos_pids[2];
PID_t xy_vel_pids[2]; PID_t xy_vel_pids[2];
@@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else { } else {
/* non-manual mode, project global setpoints to local frame */ /* non-manual mode, project global setpoints to local frame */
//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (local_pos.home_timestamp != local_home_timestamp) { if (local_pos.ref_timestamp != local_ref_timestamp) {
local_home_timestamp = local_pos.home_timestamp; local_ref_timestamp = local_pos.ref_timestamp;
/* init local projection using local position home */ /* init local projection using local position home */
double lat_home = local_pos.home_lat * 1e-7; double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.home_lon * 1e-7; double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home); map_projection_init(lat_home, lon_home);
warnx("local pos home: lat = %.10f, lon = %.10f", 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); 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; local_pos_sp.z = -global_pos_sp.altitude;
} else { } 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); 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 */ /* manual control - move setpoints */
if (control_mode.flag_control_manual_enabled) { 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) { if (home_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */ /* 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_t = local_pos.ref_timestamp;
home_alt = local_pos.home_alt; home_alt = local_pos.ref_alt;
} }
if (control_mode.flag_control_altitude_enabled) { if (control_mode.flag_control_altitude_enabled) {
src/modules/position_estimator_inav/position_estimator_inav_main.c
+102 -93
View File
@@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false; static bool verbose_mode = false;
static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s 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[]); __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; int baro_init_num = 200;
float baro_alt0 = 0.0f; /* to determine while start up */ 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 accel_counter = 0;
uint32_t baro_counter = 0; uint32_t baro_counter = 0;
@@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* first parameters update */ /* first parameters update */
parameters_update(&pos_inav_param_handles, &params); parameters_update(&pos_inav_param_handles, &params);
struct pollfd fds_init[2] = { struct pollfd fds_init[1] = {
{ .fd = sensor_combined_sub, .events = POLLIN }, { .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 */ /* wait for initial baro value */
bool wait_gps = params.use_gps;
bool wait_baro = true; 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; thread_running = true;
while ((wait_gps || wait_baro) && !thread_should_exit) { while (wait_baro && !thread_should_exit) {
int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); int ret = poll(fds_init, 1, 1000);
if (ret < 0) { if (ret < 0) {
/* poll error */ /* poll error */
@@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
baro_alt0 /= (float) baro_init_cnt; baro_alt0 /= (float) baro_init_cnt;
warnx("init baro: alt = %.3f", baro_alt0); warnx("init baro: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] 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.ref_alt = baro_alt0;
local_pos.home_timestamp = hrt_absolute_time(); local_pos.ref_timestamp = hrt_absolute_time();
} local_pos.z_valid = true;
} local_pos.v_z_valid = true;
} local_pos.global_z = true;
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);
} }
} }
} }
} }
} }
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; hrt_abstime t_prev = 0;
uint16_t accel_updates = 0; uint16_t accel_updates = 0;
@@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
while (!thread_should_exit) { 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(); hrt_abstime t = hrt_absolute_time();
if (ret < 0) { if (ret < 0) {
@@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
baro_alt0 += sonar_corr; baro_alt0 += sonar_corr;
warnx("new home: alt = %.3f", baro_alt0); warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] 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.ref_alt = baro_alt0;
local_pos.home_timestamp = hrt_absolute_time(); local_pos.ref_timestamp = hrt_absolute_time();
z_est[0] += sonar_corr; z_est[0] += sonar_corr;
sonar_corr = 0.0f; sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f; sonar_corr_filtered = 0.0f;
@@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_updates++; flow_updates++;
} }
if (params.use_gps && (fds[5].revents & POLLIN)) { if (fds[5].revents & POLLIN) {
/* vehicle GPS position */ /* vehicle GPS position */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) { if (gps.fix_type >= 3) {
/* project GPS lat lon to plane */ /* initialize reference position if needed */
float gps_proj[2]; if (ref_xy_inited) {
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); /* project GPS lat lon to plane */
gps_corr[0][0] = gps_proj[0] - x_est[0]; float gps_proj[2];
gps_corr[1][0] = gps_proj[1] - y_est[0]; 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) { /* calculate correction for velocity */
gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; if (gps.vel_ned_valid) {
gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; 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 { } else {
gps_corr[0][1] = 0.0f; hrt_abstime t = hrt_absolute_time();
gps_corr[1][1] = 0.0f;
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++; gps_updates++;
} else { } else {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr)); 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(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); 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 gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
bool can_estimate_pos = params.use_gps && 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 prediction for position */
inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_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[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(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); if (gps_valid) {
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); 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) { 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[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); 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) { if (t - pub_last > pub_interval) {
pub_last = t; pub_last = t;
/* publish local position */
local_pos.timestamp = t; 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.x = x_est[0];
local_pos.vx = x_est[1]; local_pos.vx = x_est[1];
local_pos.y = y_est[0]; local_pos.y = y_est[0];
local_pos.vy = y_est[1]; local_pos.vy = y_est[1];
local_pos.z = z_est[0]; local_pos.z = z_est[0];
local_pos.vz = z_est[1]; local_pos.vz = z_est[1];
local_pos.absolute_alt = local_pos.home_alt - local_pos.z; local_pos.landed = false; // TODO
local_pos.hdg = att.yaw;
if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
&& (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);
if (params.use_gps) { /* publish global position */
double est_lat, est_lon; global_pos.valid = local_pos.global_xy;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); if (local_pos.global_xy) {
double est_lat, est_lon;
global_pos.valid = local_pos.valid; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.timestamp = t; global_pos.lat = (int32_t)(est_lat * 1e7);
global_pos.time_gps_usec = gps.time_gps_usec; global_pos.lon = (int32_t)(est_lon * 1e7);
global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.time_gps_usec = gps.time_gps_usec;
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);
}
} }
/* 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);
} }
} }
src/modules/position_estimator_inav/position_estimator_inav_params.c
-3
View File
@@ -40,7 +40,6 @@
#include "position_estimator_inav_params.h" #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_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.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) 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_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC"); h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); 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) 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_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc)); param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_alt_sonar, &(p->w_alt_sonar)); param_get(h->w_alt_sonar, &(p->w_alt_sonar));
src/modules/position_estimator_inav/position_estimator_inav_params.h
-2
View File
@@ -41,7 +41,6 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
struct position_estimator_inav_params { struct position_estimator_inav_params {
int use_gps;
float w_alt_baro; float w_alt_baro;
float w_alt_acc; float w_alt_acc;
float w_alt_sonar; float w_alt_sonar;
@@ -56,7 +55,6 @@ struct position_estimator_inav_params {
}; };
struct position_estimator_inav_param_handles { struct position_estimator_inav_param_handles {
param_t use_gps;
param_t w_alt_baro; param_t w_alt_baro;
param_t w_alt_acc; param_t w_alt_acc;
param_t w_alt_sonar; param_t w_alt_sonar;
src/modules/sdlog2/sdlog2.c
+3 -4
View File
@@ -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.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy; 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.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
LOGBUFFER_WRITE_AND_COUNT(LPOS); LOGBUFFER_WRITE_AND_COUNT(LPOS);
} }
src/modules/sdlog2/sdlog2_messages.h
+4 -5
View File
@@ -106,10 +106,9 @@ struct log_LPOS_s {
float vx; float vx;
float vy; float vy;
float vz; float vz;
float hdg; int32_t ref_lat;
int32_t home_lat; int32_t ref_lon;
int32_t home_lon; float ref_alt;
float home_alt;
}; };
/* --- LPSP - LOCAL POSITION SETPOINT --- */ /* --- 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(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), 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(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
src/modules/uORB/topics/vehicle_local_position.h
+19 -20
View File
@@ -54,27 +54,26 @@
*/ */
struct vehicle_local_position_s struct vehicle_local_position_s
{ {
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
bool valid; /**< true if position satisfies validity criteria of estimator */ bool xy_valid; /**< true if x and y are valid */
bool z_valid; /**< true if z is valid */
float x; /**< X positin in meters in NED earth-fixed frame */ bool v_xy_valid; /**< true if vy and vy are valid */
float y; /**< X positin in meters in NED earth-fixed frame */ bool v_z_valid; /**< true if vz is valid */
float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ /* Position in local NED frame */
float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ float x; /**< X position in meters in NED earth-fixed frame */
float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ float y; /**< X position in meters in NED earth-fixed frame */
float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ /* Velocity in NED frame */
float hdg; /**< Compass heading in radians -PI..+PI. */ float vx; /**< Ground X Speed (Latitude), m/s in NED */
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
// TODO Add covariances here float vz; /**< Ground Z Speed (Altitude), m/s in NED */
/* Reference position in GPS / WGS84 frame */ /* Reference position in GPS / WGS84 frame */
uint64_t home_timestamp;/**< Time when home position was set */ bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */
int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ uint64_t ref_timestamp; /**< Time when reference position was set */
float home_alt; /**< Altitude in meters LOGME */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
float home_hdg; /**< Compass heading in radians -PI..+PI. */ 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 */ bool landed; /**< true if vehicle is landed */
}; };
src/modules/uORB/topics/vehicle_status.h
+2 -2
View File
@@ -68,8 +68,7 @@ typedef enum {
/* navigation state machine */ /* navigation state machine */
typedef enum { typedef enum {
NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization
NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
NAVIGATION_STATE_STABILIZE, // attitude stabilization NAVIGATION_STATE_STABILIZE, // attitude stabilization
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position 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_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_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_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_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 */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */