fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3203 from ChristophTobler/master

Browse Source 
changes needed for fake gps
This commit is contained in:
Lorenz Meier 2015-11-18 10:29:18 +01:00
commit 090fef8ea3
7 changed files with 43 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -564,6 +564,10 @@ then
then
mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
fi
if param compare SYS_COMPANION 257600
then
mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
fi
# Sensors on the PWM interface bank
# clear pins 5 and 6
if param compare SENS_EN_LL40LS 1

9
src/modules/mavlink/mavlink_main.cpp
View File

@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
} else if (strcmp(myoptarg, "magic") == 0) {
_mode = MAVLINK_MODE_MAGIC;
} else if (strcmp(myoptarg, "config") == 0) {
_mode = MAVLINK_MODE_CONFIG;
}
@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SYSTEM_TIME", 1.0f);
configure_stream("TIMESYNC", 10.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
/* camera trigger is rate limited at the source, do not limit here */
//camera trigger is rate limited at the source, do not limit here
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("EXTENDED_SYS_STATE", 2.0f);
break;
@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("EXTENDED_SYS_STATE", 1.0f);
break;
case MAVLINK_MODE_MAGIC:
//stream nothing
break;
case MAVLINK_MODE_CONFIG:
// Enable a number of interesting streams we want via USB
configure_stream("SYS_STATUS", 1.0f);

1
src/modules/mavlink/mavlink_main.h
View File

@ -149,6 +149,7 @@ public:
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD,
MAVLINK_MODE_OSD,
MAVLINK_MODE_MAGIC,
MAVLINK_MODE_CONFIG
};

2
src/modules/position_estimator_inav/CMakeLists.txt
View File

@ -31,7 +31,7 @@
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
endif()
px4_add_module(
MODULE modules__position_estimator_inav

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

@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
//this flag is not working -->
flow_accurate = true; //already checked if flow_q > 0.3
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
@ -840,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
@ -953,9 +951,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* increase EPH/EPV on each step */
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
eph = 0.001;
}
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 0.001;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
@ -968,9 +972,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
if(params.disable_mocap) { //disable mocap if fake gps is used
use_mocap = false;
}
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
@ -1166,7 +1174,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
}
if (use_gps_xy) {
@ -1264,6 +1271,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;

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

@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
* Weight (cutoff frequency) for optical flow (velocity) measurements.
*
* @min 0.0
* @max 30.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f);
/**
* XY axis weight for resetting velocity
@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
/**
* Disable mocap (set 0 if using fake gps)
*
* Disable mocap
*
* @min 0
* @max 1
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);
/**
* Disable vision input
*
@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
h->delay_gps = param_find("INAV_DELAY_GPS");
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
h->disable_mocap = param_find("INAV_DISAB_MOCAP");
return 0;
}
@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
param_get(h->delay_gps, &(p->delay_gps));
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
param_get(h->disable_mocap, &(p->disable_mocap));
return 0;
}

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

@ -67,6 +67,7 @@ struct position_estimator_inav_params {
float delay_gps;
float flow_module_offset_x;
float flow_module_offset_y;
int32_t disable_mocap;
};
struct position_estimator_inav_param_handles {
@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles {
param_t delay_gps;
param_t flow_module_offset_x;
param_t flow_module_offset_y;
param_t disable_mocap;
};
#define CBRK_NO_VISION_KEY 328754