fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 06:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'ekf_params' of github.com:PX4/Firmware into ekf_params

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-20 03:43:37 +02:00
parent b37d0f8f2e 7cad27a024
commit 200bd8e3dd
2 changed files with 19 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+3 -3
View File
@@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[])
&& global_position.global_valid) { && global_position.global_valid) {
/* copy position data to uORB home message, store it locally as well */ /* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat; home.lat = gps_position.lat / (double)1e7;
home.lon = global_position.lon; home.lon = gps_position.lon / (double)1e7;
home.alt = global_position.alt; home.alt = gps_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+16 -6
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h> #include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h> #include <geo/geo.h>
@@ -152,6 +153,7 @@ private:
int _params_sub; /**< notification of parameter updates */ int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */ int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub; int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _global_pos_pub; /**< global position */
@@ -477,6 +479,7 @@ FixedwingEstimator::task_main()
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
@@ -893,20 +896,27 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&start_time) > 100000) { if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && (_ekf->GPSstatus == 3)) { bool home_set;
orb_check(_home_sub, &home_set);
if (home_set && !_gps_initialized && _gps.fix_type > 2) {
_ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s; _ekf->velNED[2] = _gps.vel_d_m_s;
double lat = _gps.lat * 1e-7; struct home_position_s home;
double lon = _gps.lon * 1e-7;
float alt = _gps.alt * 1e-3; orb_copy(ORB_ID(home_position), _home_sub, &home);
double lat = home.lat;
double lon = home.lon;
float alt = home.alt;
_ekf->InitialiseFilter(_ekf->velNED); _ekf->InitialiseFilter(_ekf->velNED);
// Initialize projection // Initialize projection
_local_pos.ref_lat = _gps.lat; _local_pos.ref_lat = home.lat * 1e7;
_local_pos.ref_lon = _gps.lon; _local_pos.ref_lon = home.lon * 1e7;
_local_pos.ref_alt = alt; _local_pos.ref_alt = alt;
_local_pos.ref_timestamp = _gps.timestamp_position; _local_pos.ref_timestamp = _gps.timestamp_position;