From 9be27ad2dea35c611abdb143750b8e8ea2090b12 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 18:40:46 +0100 Subject: [PATCH] [modules][messages] send pos as LLA in DC_SHOT and DC_INFO --- conf/messages.xml | 12 +++---- sw/airborne/modules/digital_cam/dc.c | 16 ++++----- sw/airborne/modules/digital_cam/hackhd.c | 8 ++--- sw/ground_segment/cockpit/live.ml | 25 +++++++++------ sw/tools/process_exif/process_exif.py | 41 ++++++++++++++++-------- 5 files changed, 59 insertions(+), 43 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index c6f245ce43e..e324b276f4e 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -528,8 +528,9 @@ - - + + + @@ -881,10 +882,9 @@ - - - - + + + diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 1a77301b4ff..d21d96df2f2 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -89,10 +89,6 @@ void dc_send_shot_position(void) { int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f); int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f); - struct UtmCoor_f * utm = stateGetPositionUtm_f(); - // east and north UTM position in cm - int32_t east = utm->east * 100; - int32_t north = utm->north * 100; // course in decideg int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10; // ground speed in cm/s @@ -107,10 +103,9 @@ void dc_send_shot_position(void) DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice, &photo_nr, - &east, - &north, - &utm->alt, - &utm->zone, + &stateGetPositionLla_i()->lat, + &stateGetPositionLla_i()->lon, + &stateGetPositionLla_i()->alt, &phi, &theta, &course, @@ -125,8 +120,9 @@ uint8_t dc_info(void) { int16_t mode = dc_autoshoot; DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice, &mode, - &stateGetPositionEnu_f()->x, - &stateGetPositionEnu_f()->y, + &stateGetPositionLla_i()->lat, + &stateGetPositionLla_i()->lon, + &stateGetPositionLla_i()->alt, &course, &dc_buffer, &dc_gps_dist, diff --git a/sw/airborne/modules/digital_cam/hackhd.c b/sw/airborne/modules/digital_cam/hackhd.c index 9e171b9e159..7eb4abe5693 100644 --- a/sw/airborne/modules/digital_cam/hackhd.c +++ b/sw/airborne/modules/digital_cam/hackhd.c @@ -92,15 +92,13 @@ static inline void hackhd_send_shot_position(void) { int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f); int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f); - float gps_z = ((float)gps.hmsl) / 1000.0f; int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice, &hackhd.photo_nr, - &gps.utm_pos.east, - &gps.utm_pos.north, - &gps_z, - &gps.utm_pos.zone, + &stateGetPositionLla_i()->lat, + &stateGetPositionLla_i()->lon, + &stateGetPositionLla_i()->alt, &phi, &theta, &course, diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 9425998709f..7787fa11aaf 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1349,15 +1349,22 @@ let message_request = Ground_Pprz.message_req let mark_dcshot = fun (geomap:G.widget) _sender vs -> let ac = find_ac !active_ac in let photonumber = Pprz.string_assoc "photo_nr" vs in - (* let ac = get_ac vs in *) - match ac.track#last with - Some geo -> - begin - let group = geomap#background in - let point = geomap#photoprojection ~group ~fill_color:"yellow" ~number:photonumber geo 3. in - point#raise_to_top () - end - | None -> () + try + let lat = Pprz.int_assoc "lat" vs + and lon = Pprz.int_assoc "lon" vs in + let wgs84 = LL.make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in + let group = geomap#background in + let point = geomap#photoprojection ~group ~fill_color:"yellow" ~number:photonumber wgs84 3. in + point#raise_to_top () + with _ -> + match ac.track#last with + Some geo -> + begin + let group = geomap#background in + let point = geomap#photoprojection ~group ~fill_color:"yellow" ~number:photonumber geo 3. in + point#raise_to_top () + end + | None -> () (* mark geomap ac.ac_name track !Plugin.frame *) diff --git a/sw/tools/process_exif/process_exif.py b/sw/tools/process_exif/process_exif.py index 31088f5097a..cb62fcb9645 100644 --- a/sw/tools/process_exif/process_exif.py +++ b/sw/tools/process_exif/process_exif.py @@ -119,20 +119,35 @@ def UTMtoLL( northing, easting, utm_zone ): # 618.710 1 DC_SHOT 212 29133350 -89510400 8.5 25 -9 29 0 0 385051650 splitted = line.split( ' ' ) - if len(splitted) < 12: - continue try: - photonr = int(splitted[ 3 ]) - utm_east = ( float(int(splitted[ 4 ])) / 100. ) - utm_north = ( float(int(splitted[ 5 ])) / 100. ) - alt = float(splitted[ 6 ]) - utm_zone = int(splitted[ 7 ]) - phi = int(splitted[ 8 ]) - theta = int(splitted[ 9 ]) - course = int(splitted[ 10 ]) - speed = int(splitted[ 11 ]) - - lon, lat = UTMtoLL( utm_north, utm_east, utm_zone ) + # old DC_SHOT message has 10 data fields with pos in UTM: + # photo_nr, utm_east, utm_north, z, utm_zone, phi, theta, course, speed, itow + if len(splitted) == 13: + photonr = int(splitted[ 3 ]) + utm_east = ( float(int(splitted[ 4 ])) / 100. ) + utm_north = ( float(int(splitted[ 5 ])) / 100. ) + alt = float(splitted[ 6 ]) + utm_zone = int(splitted[ 7 ]) + phi = int(splitted[ 8 ]) + theta = int(splitted[ 9 ]) + course = int(splitted[ 10 ]) + speed = int(splitted[ 11 ]) + + lon, lat = UTMtoLL( utm_north, utm_east, utm_zone ) + + # current DC_SHOT messages has 9 data fields with pos in LLA: + # photo_nr, latitude, longitude, altitude, phi, theta, course, speed, itow + else if len(splitted) == 12: + photonr = int(splitted[ 3 ]) + lat = RadOfDeg(int(splitted[ 4 ]) * 0.0000001) # to radians + lon = RadOfDeg(int(splitted[ 5 ]) * 0.0000001) # to radians + alt = int(splitted[ 6 ]) * 0.001 # to meters + phi = int(splitted[ 7 ]) + theta = int(splitted[ 8 ]) + course = int(splitted[ 9 ]) + speed = int(splitted[ 10 ]) + else: + continue # Check that there as many photos and pick the indicated one. # (this assumes the photos were taken correctly without a hiccup)