dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: "landed" detector implemented, bugfixes

This commit is contained in:
Anton Babushkin 2013-08-20 12:17:15 +02:00
parent de124619b6
commit db950f7489
6 changed files with 118 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/commander/commander.cpp
View File

@ -820,6 +820,17 @@ int commander_thread_main(int argc, char *argv[])
/* 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);
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
if (status.condition_landed) {
mavlink_log_info(mavlink_fd, "[cmd] LANDED");
} else {
mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
}
}
}
/* update battery status */
orb_check(battery_sub, &updated);

4
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -459,6 +459,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* publish local position setpoint as projection of global position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
}
/* reset setpoints after non-manual modes */
reset_sp_xy = true;
reset_sp_z = true;
}
/* run position & altitude controllers, calculate velocity setpoint */

4
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@ -41,8 +41,8 @@
#include "multirotor_pos_control_params.h"
/* controller parameters */
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f);
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);

126
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -52,10 +52,11 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
@ -169,14 +170,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_alt0 = 0.0f; /* to determine while start up */
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
/* make sure that baroINITdone = false */
struct actuator_controls_effective_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
@ -192,7 +197,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
@ -294,9 +300,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime sonar_time = 0;
/* main loop */
struct pollfd fds[6] = {
struct pollfd fds[7] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = vehicle_status_sub, .events = POLLIN },
{ .fd = actuator_sub, .events = POLLIN },
{ .fd = armed_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
@ -308,7 +315,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
while (!thread_should_exit) {
int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate
int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
@ -328,20 +335,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
parameters_update(&pos_inav_param_handles, &params);
}
/* vehicle status */
/* actuator */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
&vehicle_status);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
}
/* armed */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* vehicle attitude */
if (fds[2].revents & POLLIN) {
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */
if (fds[3].revents & POLLIN) {
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) {
@ -378,7 +389,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* optical flow */
if (fds[4].revents & POLLIN) {
if (fds[5].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
@ -415,33 +426,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_updates++;
}
if (fds[5].revents & POLLIN) {
/* vehicle GPS position */
if (fds[6].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) {
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
/* 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];
/* 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 {
hrt_abstime t = hrt_absolute_time();
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
@ -462,12 +453,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
gps_updates++;
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];
/* 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 {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr));
ref_xy_init_start = 0;
}
gps_updates++;
}
}
@ -516,9 +527,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
/* detect land */
alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp = z_est[0] - alt_avg;
alt_disp = alt_disp * alt_disp;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
} else {
if (alt_disp < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
landed_time = t; // land detected first time
} else {
if (t > landed_time + params.land_t * 1000000.0f) {
landed = true;
landed_time = 0;
}
}
} else {
landed_time = 0;
}
}
if (verbose_mode) {
/* print updates rate */
if (t - updates_counter_start > updates_counter_len) {
if (t > updates_counter_start + updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
@ -536,7 +578,7 @@ 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;
/* publish local position */
local_pos.timestamp = t;
@ -549,7 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.landed = false; // TODO
local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);

11
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -40,7 +40,7 @@
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
@ -51,6 +51,9 @@ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
@ -65,6 +68,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->flow_k = param_find("INAV_FLOW_K");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
h->land_t = param_find("INAV_LAND_T");
h->land_disp = param_find("INAV_LAND_DISP");
h->land_thr = param_find("INAV_LAND_THR");
return OK;
}
@ -82,6 +88,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->flow_k, &(p->flow_k));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
param_get(h->land_t, &(p->land_t));
param_get(h->land_disp, &(p->land_disp));
param_get(h->land_thr, &(p->land_thr));
return OK;
}

6
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -52,6 +52,9 @@ struct position_estimator_inav_params {
float flow_k;
float sonar_filt;
float sonar_err;
float land_t;
float land_disp;
float land_thr;
};
struct position_estimator_inav_param_handles {
@ -66,6 +69,9 @@ struct position_estimator_inav_param_handles {
param_t flow_k;
param_t sonar_filt;
param_t sonar_err;
param_t land_t;
param_t land_disp;
param_t land_thr;
};
/**