diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-27 16:16:57 -0800 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-27 16:16:57 -0800 |
commit | d71396e556a4a729d7fdf14244c1592477cb399e (patch) | |
tree | 3d8728a541c714d42df14d2e2f12e3b0fb920fcd /src/modules/fw_att_pos_estimator | |
parent | dac0427b602be5de9058e78851a9182fd236ce0a (diff) | |
download | px4-firmware-d71396e556a4a729d7fdf14244c1592477cb399e.tar.gz px4-firmware-d71396e556a4a729d7fdf14244c1592477cb399e.tar.bz2 px4-firmware-d71396e556a4a729d7fdf14244c1592477cb399e.zip |
Move params to MAVLink params, enable mavlink text feedback.
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 52 |
1 files changed, 27 insertions, 25 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index bb3541a39..7e179d400 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -77,6 +77,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> #include "estimator.h" @@ -174,18 +175,22 @@ private: bool _initialized; bool _gps_initialized; + int _mavlink_fd; + struct { - float throttle_cruise; - uint32_t vel_delay_ms; - uint32_t pos_delay_ms; - uint32_t height_delay_ms; - uint32_t mag_delay_ms; - uint32_t tas_delay_ms; + int32_t vel_delay_ms; + int32_t pos_delay_ms; + int32_t height_delay_ms; + int32_t mag_delay_ms; + int32_t tas_delay_ms; } _parameters; /**< local copies of interesting parameters */ struct { - param_t throttle_cruise; - + param_t vel_delay_ms; + param_t pos_delay_ms; + param_t height_delay_ms; + param_t mag_delay_ms; + param_t tas_delay_ms; } _parameter_handles; /**< handles for interesting parameters */ @@ -264,10 +269,17 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _initialized(false), - _gps_initialized(false) + _gps_initialized(false), + _mavlink_fd(-1) { - _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); + _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); + _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); + _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); + _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); /* fetch initial parameter values */ parameters_update(); @@ -327,14 +339,11 @@ int FixedwingEstimator::parameters_update() { - // XXX NEED TO GET HANDLES FIRST! NEEDS PARAM INIT - //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); - - _parameters.vel_delay_ms = 230; - _parameters.pos_delay_ms = 210; - _parameters.height_delay_ms = 350; - _parameters.mag_delay_ms = 30; - _parameters.tas_delay_ms = 210; + param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); + param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); + param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); + param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); + param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); return OK; } @@ -361,13 +370,6 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) float dt = 0.0f; // time lapsed since last covariance prediction -// Estimated time delays (msec) -uint32_t msecVelDelay = 230; -uint32_t msecPosDelay = 210; -uint32_t msecHgtDelay = 350; -uint32_t msecMagDelay = 30; -uint32_t msecTasDelay = 210; - void FixedwingEstimator::task_main() { |