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

global_position topic: added baro_alt, mc_pos_control: SEATBELT mode fixed, use baro/AMSL alt

Browse Source
This commit is contained in:
Anton Babushkin 2014-01-28 20:40:05 +01:00
parent 48f1b7e1c7
commit 6a1a29f77e
11 changed files with 69 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -225,7 +225,7 @@ void frsky_send_frame2(int uart)
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos.valid) {
if (global_pos.global_valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);

2
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
View File

@ -314,7 +314,7 @@ void KalmanNav::updatePublications()
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
_pos.global_valid = true;
_pos.lat = getLatDegE7();
_pos.lon = getLonDegE7();
_pos.alt = float(alt);

2
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;

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

@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
/* update local position estimate */
orb_check(local_position_sub, &updated);
@ -1030,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
&& global_position.global_valid) {
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg)
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.global_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;

45
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -182,6 +182,7 @@ private:
bool _reset_lat_lon_sp;
bool _reset_alt_sp;
bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
@ -214,6 +215,11 @@ private:
*/
void reset_alt_sp();
/**
* Select between barometric and global (AMSL) altitudes
*/
void select_alt(bool global);
/**
* Shim for calling task_main from task_create.
*/
@ -263,7 +269,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_alt_sp(0.0f),
_reset_lat_lon_sp(true),
_reset_alt_sp(true)
_reset_alt_sp(true),
_use_global_alt(false)
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
@ -466,8 +473,23 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_alt_sp = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp);
_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
}
}
void
MulticopterPositionControl::select_alt(bool global)
{
if (global != _use_global_alt) {
_use_global_alt = global;
if (global) {
/* switch from barometric to global altitude */
_alt_sp += _global_pos.alt - _global_pos.baro_alt;
} else {
/* switch from global to barometric altitude */
_alt_sp += _global_pos.baro_alt - _global_pos.alt;
}
}
}
@ -565,8 +587,16 @@ MulticopterPositionControl::task_main()
sp_move_rate.zero();
float alt = _global_pos.alt;
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* select altitude source and update setpoint */
select_alt(_global_pos.global_valid);
if (!_use_global_alt) {
alt = _global_pos.baro_alt;
}
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
@ -612,7 +642,7 @@ MulticopterPositionControl::task_main()
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2);
pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
@ -620,7 +650,7 @@ MulticopterPositionControl::task_main()
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
_alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2);
_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
}
/* fill position setpoint triplet */
@ -647,6 +677,9 @@ MulticopterPositionControl::task_main()
}
} else if (_control_mode.flag_control_auto_enabled) {
/* always use AMSL altitude for AUTO */
select_alt(true);
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
@ -678,7 +711,7 @@ MulticopterPositionControl::task_main()
math::Vector<3> pos_err;
float err_x, err_y;
get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
pos_err(2) = -(_alt_sp - _global_pos.alt);
pos_err(2) = -(_alt_sp - alt);
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);

5
src/modules/navigator/navigator_main.cpp
View File

@ -840,6 +840,7 @@ Navigator::task_main()
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
publish_position_setpoint_triplet();
}
@ -882,9 +883,9 @@ Navigator::start()
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
if (_global_pos.valid) {
if (_global_pos.global_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));

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

@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
global_pos.baro_valid = true;
}
}
}
@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
flag_armed = armed.armed;
baro_offset -= z_est[0];
corr_baro = 0.0f;
local_pos.ref_alt -= z_est[0];
local_pos.ref_timestamp = t;
z_est[0] = 0.0f;
alt_avg = 0.0f;
}
}
/* sensor combined */
@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.005);
t_prev = t;
/* use GPS if it's valid and reference position initialized */
@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
baro_counter += offs_corr;
corr_baro += offs_corr;
}
/* accelerometer bias correction */
@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
/* publish global position */
global_pos.valid = local_pos.xy_global;
global_pos.global_valid = local_pos.xy_global;
if (local_pos.xy_global) {
double est_lat, est_lon;
@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.alt = local_pos.ref_alt - local_pos.z;
}
if (local_pos.z_valid) {
global_pos.baro_alt = baro_offset - local_pos.z;
}
if (local_pos.v_z_valid) {
global_pos.vel_d = local_pos.vz;
}

2
src/modules/sdlog2/sdlog2.c
View File

@ -1244,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}

4
src/modules/sdlog2/sdlog2_messages.h
View File

@ -204,6 +204,8 @@ struct log_GPOS_s {
float vel_n;
float vel_e;
float vel_d;
float baro_alt;
uint8_t flags;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
@ -303,7 +305,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),

12
src/modules/uORB/topics/vehicle_global_position.h
View File

@ -61,17 +61,21 @@
*/
struct vehicle_global_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
bool global_valid; /**< true if position satisfies validity criteria of estimator */
bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float alt; /**< Altitude AMSL in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */
};
/**