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

Merge branch 'master' into mavlink_fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-17 09:27:24 +04:00
parent 6cf09183d6 39634d1001
commit bb9f67ca76
6 changed files with 290 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+51 -18
View File
@@ -41,6 +41,7 @@
#include "KalmanNav.hpp"
#include <systemlib/err.h>
#include <geo/geo.h>
// constants
// Titterton pg. 52
@@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// attitude representations
C_nb(),
q(),
_accel_sub(-1),
_gyro_sub(-1),
_mag_sub(-1),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
@@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
@@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
lon = 0.0f;
alt = 0.0f;
// gyro, accel and mag subscriptions
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
struct accel_report accel;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
// initialize rotation quaternion with a single raw sensor measurement
q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
_sensors.update();
q = init(
_sensors.accelerometer_m_s2[0],
_sensors.accelerometer_m_s2[1],
_sensors.accelerometer_m_s2[2],
_sensors.magnetometer_ga[0],
_sensors.magnetometer_ga[1],
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
@@ -252,11 +248,21 @@ void KalmanNav::update()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// set reference position for
// local position
lat0 = lat;
lon0 = lon;
alt0 = alt;
// XXX map_projection has internal global
// states that multiple things could change,
// should make map_projection take reference
// lat/lon and not have init
map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
lat, lon, double(alt));
}
// prediction step
@@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
{
using namespace math;
// position publication
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
@@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
_pos.vz = vD;
_pos.yaw = psi;
// local position publication
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
map_projection_project(lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
_localPos.v_xy_valid = true;
_localPos.v_z_valid = true;
_localPos.x = x;
_localPos.y = y;
_localPos.z = alt0 - alt;
_localPos.vx = vN;
_localPos.vy = vE;
_localPos.vz = vD;
_localPos.yaw = psi;
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = getLatDegE7();
_localPos.ref_lon = getLonDegE7();
_localPos.ref_alt = 0;
_localPos.landed = landed;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
@@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized)
if (_positionInitialized) {
_pos.update();
_localPos.update();
}
if (_attitudeInitialized)
_att.update();
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+5 -6
View File
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
@@ -142,12 +143,8 @@ protected:
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
int _accel_sub; /**< Accelerometer subscription */
int _gyro_sub; /**< Gyroscope subscription */
int _mag_sub; /**< Magnetometer subscription */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -164,8 +161,10 @@ protected:
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
float alt; /**< altitude, meters */
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
src/modules/fixedwing_backside/fixedwing.cpp
+12 -11
View File
@@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
if (_status.main_state == MAIN_STATE_AUTO) {
// TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
if (_status.main_state == MAIN_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// calculate velocity, XXX should be airspeed,
// but using ground speed for now for the purpose
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
@@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update()
// a first binary release can be targeted.
// This is not a hack, but a design choice.
/* do not limit in HIL */
// do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
} else if (_status.main_state == MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
src/modules/fixedwing_backside/fixedwing.hpp
+7 -7
View File
@@ -255,13 +255,13 @@ private:
BlockWaypointGuidance _guide;
// block params
BlockParam<float> _trimAil;
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _crMax;
BlockParamFloat _trimAil;
BlockParamFloat _trimElv;
BlockParamFloat _trimRdr;
BlockParamFloat _trimThr;
BlockParamFloat _trimV;
BlockParamFloat _vCmd;
BlockParamFloat _crMax;
struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd;
src/modules/mavlink/mavlink_receiver.cpp
+55 -10
View File
@@ -72,6 +72,7 @@
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
#include <geo/geo.h>
__BEGIN_DECLS
@@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
static struct offboard_control_setpoint_s offboard_control_sp;
struct vehicle_global_position_s hil_global_pos;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_local_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
@@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
static orb_advert_t telemetry_status_pub = -1;
// variables for HIL reference position
static int32_t lat0 = 0;
static int32_t lon0 = 0;
static double alt0 = 0;
static void
handle_message(mavlink_message_t *msg)
{
@@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
/* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time();
uint64_t timestamp = hrt_absolute_time();
// publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
// publish local position
if (pub_hil_local_pos > 0) {
float x;
float y;
bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
double lat = hil_state.lat*1e-7;
double lon = hil_state.lon*1e-7;
map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
hil_local_pos.v_xy_valid = true;
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
hil_local_pos.vx = hil_state.vx/100.0f;
hil_local_pos.vy = hil_state.vy/100.0f;
hil_local_pos.vz = hil_state.vz/100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
} else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat;
lon0 = hil_state.lon;
alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
}
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);