Skip to content

Commit

Permalink
[modules][messages] send pos as LLA in DC_SHOT and DC_INFO
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Dec 3, 2014
1 parent 22f29ab commit 9be27ad
Show file tree
Hide file tree
Showing 5 changed files with 59 additions and 43 deletions.
12 changes: 6 additions & 6 deletions conf/messages.xml
View file
Expand Up @@ -528,8 +528,9 @@

<message name="DC_INFO" id="64">
<field name="mode" type="int16" unit=""/>
<field name="utm_east" type="float" unit="m"/>
<field name="utm_north" type="float" unit="m"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="course" type="float" unit="deg"/>
<field name="buffer" type="uint16"/>
<field name="dist" type="float" unit="m"/>
Expand Down Expand Up @@ -881,10 +882,9 @@

<message name="DC_SHOT" id="110">
<field name="photo_nr" type="int16"/>
<field name="utm_east" type="int32" unit="cm"/>
<field name="utm_north" type="int32" unit="cm"/>
<field name="z" type="float" unit="m"/>
<field name="utm_zone" type="uint8"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="phi" type="int16" unit="decideg"/>
<field name="theta" type="int16" unit="decideg"/>
<field name="course" type="int16" unit="decideg"/>
Expand Down
16 changes: 6 additions & 10 deletions sw/airborne/modules/digital_cam/dc.c
View file
Expand Up @@ -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
Expand All @@ -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,
Expand All @@ -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,
Expand Down
8 changes: 3 additions & 5 deletions sw/airborne/modules/digital_cam/hackhd.c
View file
Expand Up @@ -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,
Expand Down
25 changes: 16 additions & 9 deletions sw/ground_segment/cockpit/live.ml
View file
Expand Up @@ -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 *)

Expand Down
41 changes: 28 additions & 13 deletions sw/tools/process_exif/process_exif.py
View file
Expand Up @@ -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)
Expand Down

0 comments on commit 9be27ad

Please sign in to comment.