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

Merge pull request #401 from PX4/sdlog2_LPOS 

sdlog2: position & velocity valid, postion global and landed flags added...
This commit is contained in:
Lorenz Meier 2013-09-15 22:53:33 -07:00
parent f4abcb51a1 2fbd23cf6e
commit 489e7b2f1e
4 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.ref_timestamp = hrt_absolute_time(); local_pos.ref_timestamp = hrt_absolute_time();
local_pos.z_valid = true; local_pos.z_valid = true;
local_pos.v_z_valid = true; local_pos.v_z_valid = true;
local_pos.global_z = true; local_pos.z_global = true;
} }
} }
} }
@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.timestamp = t; local_pos.timestamp = t;
local_pos.xy_valid = can_estimate_xy && gps_valid; local_pos.xy_valid = can_estimate_xy && gps_valid;
local_pos.v_xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy;
local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
local_pos.x = x_est[0]; local_pos.x = x_est[0];
local_pos.vx = x_est[1]; local_pos.vx = x_est[1];
local_pos.y = y_est[0]; local_pos.y = y_est[0];
@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
/* publish global position */ /* publish global position */
global_pos.valid = local_pos.global_xy; global_pos.valid = local_pos.xy_global;
if (local_pos.global_xy) { if (local_pos.xy_global) {
double est_lat, est_lon; double est_lat, est_lon;
map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lat = (int32_t)(est_lat * 1e7);
@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.relative_alt = -local_pos.z; global_pos.relative_alt = -local_pos.z;
} }
if (local_pos.global_z) { if (local_pos.z_global) {
global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.alt = local_pos.ref_alt - local_pos.z;
} }

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

@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS); LOGBUFFER_WRITE_AND_COUNT(LPOS);
} }

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

@ -109,6 +109,9 @@ struct log_LPOS_s {
int32_t ref_lat; int32_t ref_lat;
int32_t ref_lon; int32_t ref_lon;
float ref_alt; float ref_alt;
uint8_t xy_flags;
uint8_t z_flags;
uint8_t landed;
}; };
/* --- LPSP - LOCAL POSITION SETPOINT --- */ /* --- LPSP - LOCAL POSITION SETPOINT --- */
@ -278,7 +281,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),

4
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -70,8 +70,8 @@ struct vehicle_local_position_s
/* Heading */ /* Heading */
float yaw; float yaw;
/* Reference position in GPS / WGS84 frame */ /* Reference position in GPS / WGS84 frame */
bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */ uint64_t ref_timestamp; /**< Time when reference position was set */
int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */