From 1c7e07d8d7256e62dfc49fc9f2f1d494c3da4cb4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 26 Dec 2013 21:41:54 +0100 Subject: Topics: Move from global_position_setpoint to mission_item_triplet --- src/modules/sdlog2/sdlog2.c | 37 ++++++++++++++++++------------------ src/modules/sdlog2/sdlog2_messages.h | 11 +++++------ 2 files changed, 24 insertions(+), 24 deletions(-) (limited to 'src/modules/sdlog2') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13c..5dc325a05 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,7 +72,7 @@ #include #include #include -#include +#include #include #include #include @@ -693,7 +693,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_global_position_setpoint_s global_pos_sp; + struct mission_item_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -718,7 +718,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; - int global_pos_sp_sub; + int triplet_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -840,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - fds[fdsc_count].fd = subs.global_pos_sp_sub; + subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -1150,20 +1150,21 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; - log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; - log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; - log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; - log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; - log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; - log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; - log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; - log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; - log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; - log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7); + log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.radius = buf.triplet.current.radius; + log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ab4dc9b00..cb1393f1f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -214,13 +214,12 @@ struct log_GPSP_s { int32_t lon; float altitude; float yaw; + uint8_t nav_cmd; float loiter_radius; int8_t loiter_direction; - uint8_t nav_cmd; - float param1; - float param2; - float param3; - float param4; + float radius; + float time_inside; + float pitch_min; }; /* --- ESC - ESC STATE --- */ @@ -288,7 +287,7 @@ static const struct log_format_s log_formats[] = { 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(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), + LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), /* system-level messages, ID >= 0x80 */ -- cgit v1.2.3 From effa62937fbaab0e446a95ac1ac9447a2895594e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:14:08 +0100 Subject: Prevent some warnings for lat/lon double conversions --- src/lib/geo/geo.c | 4 ++-- src/modules/mavlink/orb_listener.c | 4 ++-- src/modules/navigator/navigator_main.cpp | 10 +++++----- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- 5 files changed, 13 insertions(+), 13 deletions(-) (limited to 'src/modules/sdlog2') diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 85b17f9ae..f64bfb41a 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -513,8 +513,8 @@ __EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, c unsigned int i, j, vertices = fence->count; bool c = false; - double lat = vehicle->lat / 1e7; - double lon = vehicle->lon / 1e7; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; // skip vertex 0 (return point) for (i = 0, j = vertices - 1; i < vertices; j = i++) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 96888f06a..17978615f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -416,8 +416,8 @@ l_global_position_setpoint(const struct listener *l) if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, - (int32_t)(triplet.current.lat * 1e7f), - (int32_t)(triplet.current.lon * 1e7f), + (int32_t)(triplet.current.lat * 1e7d), + (int32_t)(triplet.current.lon * 1e7d), (int32_t)(triplet.current.altitude * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6fe93e9e..7be9229c9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -723,7 +723,7 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", @@ -947,8 +947,8 @@ Navigator::start_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; _mission_item_triplet.current.yaw = 0.0f; get_loiter_item(&_mission_item_triplet.current); @@ -1162,11 +1162,11 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); - // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt); + // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); // warnx("Dist: %4.4f", dist); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d92..5bf0fba30 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -614,8 +614,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double 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.lon = (int32_t)(est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7d); + global_pos.lon = (int32_t)(est_lon * 1e7d); global_pos.time_gps_usec = gps.time_gps_usec; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5dc325a05..9f634d9e4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1153,8 +1153,8 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7); + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; -- cgit v1.2.3