dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

'vehicle_global_position' topic updated: removed baro_alt and XXX_valid flags.

This commit is contained in:
Anton Babushkin 2014-03-27 00:27:11 +04:00
parent e2305d93bd
commit 83da4ae02d
10 changed files with 77 additions and 90 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.global_valid) {
if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);

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

@ -312,7 +312,6 @@ void KalmanNav::updatePublications()
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.global_valid = true;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);

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

@ -407,7 +407,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.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;

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

@ -117,7 +117,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
#define POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */
#define RC_TIMEOUT 100000
#define DIFFPRESS_TIMEOUT 2000000
@ -919,7 +919,37 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
static float hdop_threshold_m = 4.0f;
static float vdop_threshold_m = 8.0f;
/* update home position */
if (!status.condition_home_position_valid && updated &&
(global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) &&
(hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) {
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.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);
/* announce new home position */
if (home_pub > 0) {
orb_publish(ORB_ID(home_position), home_pub, &home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &home);
}
/* mark home position as set */
status.condition_home_position_valid = true;
tune_positive(true);
}
/* update local position estimate */
orb_check(local_position_sub, &updated);
@ -1067,45 +1097,6 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
/* check if GPS fix is ok */
float hdop_threshold_m = 4.0f;
float vdop_threshold_m = 8.0f;
/*
* If horizontal dilution of precision (hdop / eph)
* and vertical diluation of precision (vdop / epv)
* are below a certain threshold (e.g. 4 m), AND
* home position is not yet set AND the last GPS
* GPS measurement is not older than two seconds AND
* the system is currently not armed, set home
* position to the current position.
*/
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.global_valid) {
/* copy position data to uORB home message, store it locally as well */
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.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);
/* announce new home position */
if (home_pub > 0) {
orb_publish(ORB_ID(home_position), home_pub, &home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &home);
}
/* mark home position as set */
status.condition_home_position_valid = true;
tune_positive(true);
}
}
/* start RC input check */

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

@ -765,7 +765,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
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;
@ -773,6 +772,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);

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

@ -177,7 +177,7 @@ private:
class Mission _mission;
bool _mission_item_valid; /**< current mission item valid */
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _global_pos_valid; /**< track changes of global_position */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@ -817,13 +817,11 @@ Navigator::task_main()
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true;
if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
if (myState == NAV_STATE_LAND && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
_global_pos_valid = _global_pos.global_valid;
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
@ -846,8 +844,15 @@ Navigator::task_main()
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
_global_pos_valid = true;
} else {
/* assume that global position is valid if updated in last 20ms */
_global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000;
}
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
@ -893,9 +898,9 @@ Navigator::start()
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
if (_global_pos.global_valid) {
if (_global_pos_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));

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

@ -292,7 +292,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
orb_advert_t vehicle_global_position_pub = -1;
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
@ -340,7 +340,6 @@ 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;
}
}
}
@ -550,9 +549,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* reproject position estimate to new reference */
float dx, dy;
map_projection_project(&ref, home.lat, home.lon, &dx, &dy);
est_x[0] -= dx;
est_y[0] -= dx;
est_z[0] += home.alt - local_pos.ref_alt;
x_est[0] -= dx;
y_est[0] -= dx;
z_est[0] += home.alt - local_pos.ref_alt;
}
/* update baro offset */
@ -888,40 +887,35 @@ 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.global_valid = local_pos.xy_global;
if (local_pos.xy_global && local_pos.z_global) {
/* publish global position */
global_pos.timestamp = t;
global_pos.time_gps_usec = gps.time_gps_usec;
if (local_pos.xy_global) {
double est_lat, est_lon;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.time_gps_usec = gps.time_gps_usec;
}
global_pos.alt = local_pos.ref_alt - local_pos.z;
/* set valid values even if position is not valid */
if (local_pos.v_xy_valid) {
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
}
if (local_pos.z_global) {
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;
global_pos.yaw = local_pos.yaw;
// TODO implement dead-reckoning
global_pos.eph = gps.eph_m;
global_pos.epv = gps.epv_m;
if (vehicle_global_position_pub < 0) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}

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

@ -1126,8 +1126,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);
log_msg.body.log_GPOS.eph = buf.global_pos.eph;
log_msg.body.log_GPOS.epv = buf.global_pos.epv;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}

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

@ -204,8 +204,8 @@ struct log_GPOS_s {
float vel_n;
float vel_e;
float vel_d;
float baro_alt;
uint8_t flags;
float eph;
float epv;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
@ -317,7 +317,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, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
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"),

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

@ -63,9 +63,6 @@ struct vehicle_global_position_s
{
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 */
@ -74,8 +71,8 @@ struct vehicle_global_position_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) */
float eph;
float epv;
};
/**