Skip to content

Commit

Permalink
RaspiStill: Apply gpsd info as EXIF tags (#286)
Browse files Browse the repository at this point in the history 
* RaspiStill: Apply gpsd info as EXIF tags

Applies GPS information from gpsd as EXIF tags.
Enable via "-gps" command line argument and requires libgps.so.22 when
enabled.
Only these GPS info are added as EXIF tags: GPSDateStamp, GPSTimeStamp,
GPSMeasureMode, GPSSatellites, GPSLatitude, GPSLatitudeRef,
GPSLongitude, GPSLongitudeRef, GPSAltitude, GPSAltitudeRef, GPSSpeed,
GPSSpeedRef, GPSTrack, GPSTrackRef.
  • Loading branch information
@jasaw @6by9
jasaw authored and 6by9 committed Aug 23, 2018
1 parent 25155c0 commit 4228b6c
Show file tree
Hide file tree
Showing 6 changed files with 2,687 additions and 5 deletions.
4 changes: 2 additions & 2 deletions host_applications/linux/apps/raspicam/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,14 @@ set (COMMON_SOURCES
RaspiCLI.c
RaspiPreview.c)

add_executable(raspistill ${COMMON_SOURCES} RaspiStill.c RaspiTex.c RaspiTexUtil.c tga.c ${GL_SCENE_SOURCES})
add_executable(raspistill ${COMMON_SOURCES} RaspiStill.c RaspiTex.c RaspiTexUtil.c tga.c ${GL_SCENE_SOURCES} libgps_loader.c)
add_executable(raspiyuv ${COMMON_SOURCES} RaspiStillYUV.c)
add_executable(raspivid ${COMMON_SOURCES} RaspiVid.c)
add_executable(raspividyuv ${COMMON_SOURCES} RaspiVidYUV.c)

set (MMAL_LIBS mmal_core mmal_util mmal_vc_client)

target_link_libraries(raspistill ${MMAL_LIBS} vcos bcm_host brcmGLESv2 brcmEGL m)
target_link_libraries(raspistill ${MMAL_LIBS} vcos bcm_host brcmGLESv2 brcmEGL m dl)
target_link_libraries(raspiyuv ${MMAL_LIBS} vcos bcm_host)
target_link_libraries(raspivid ${MMAL_LIBS} vcos bcm_host)
target_link_libraries(raspividyuv ${MMAL_LIBS} vcos bcm_host)
Expand Down
2 changes: 1 addition & 1 deletion host_applications/linux/apps/raspicam/Makefile
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
OBJS=RaspiCamControl.o RaspiCLI.o RaspiPreview.o RaspiStill.o
BIN=raspicam.bin
LDFLAGS+=-lmmal -lmmal_core -lmmal_util
LDFLAGS+=-lmmal -lmmal_core -lmmal_util -lm -ldl

include ../Makefile.include

266 changes: 264 additions & 2 deletions host_applications/linux/apps/raspicam/RaspiStill.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "RaspiCLI.h"
#include "RaspiTex.h"

#include "libgps_loader.h"

#include <semaphore.h>
#include <math.h>
#include <pthread.h>
#include <time.h>

// Standard port setting for the camera component
#define MMAL_CAMERA_PREVIEW_PORT 0
Expand Down Expand Up @@ -147,6 +152,7 @@ typedef struct
int datetime; /// Use DateTime instead of frame#
int timestamp; /// Use timestamp instead of frame#
int restart_interval; /// JPEG restart interval. 0 for none.
int gpsdExif; /// Add real-time gpsd output as EXIF tags

RASPIPREVIEW_PARAMETERS preview_parameters; /// Preview setup parameters
RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters
Expand All @@ -172,6 +178,21 @@ typedef struct
RASPISTILL_STATE *pstate; /// pointer to our state in case required in callback
} PORT_USERDATA;

typedef struct
{
pthread_mutex_t gps_cache_mutex;
gpsd_info gpsd;
struct gps_data_t gpsdata_cache;
time_t last_valid_time;
pthread_t gps_reader_thread;
RASPISTILL_STATE *pstate; /// pointer to our state
int terminated;
int gps_reader_thread_ok;
} GPS_READER_DATA;
static GPS_READER_DATA gps_reader_data;

#define GPS_CACHE_EXPIRY 5 // in seconds

static void display_valid_parameters(char *app_name);
static void store_exif_tag(RASPISTILL_STATE *state, const char *exif_tag);

Expand Down Expand Up @@ -203,6 +224,7 @@ static void store_exif_tag(RASPISTILL_STATE *state, const char *exif_tag);
#define CommandTimeStamp 24
#define CommandFrameStart 25
#define CommandRestartInterval 26
#define CommandGpsdExif 27

static COMMAND_LIST cmdline_commands[] =
{
Expand Down Expand Up @@ -233,6 +255,7 @@ static COMMAND_LIST cmdline_commands[] =
{ CommandTimeStamp, "-timestamp", "ts", "Replace output pattern (%d) with unix timestamp (seconds since 1970)", 0},
{ CommandFrameStart,"-framestart","fs", "Starting frame number in output pattern(%d)", 1},
{ CommandRestartInterval, "-restart","rs","JPEG Restart interval (default of 0 for none)", 1},
{ CommandGpsdExif, "-gpsdexif", "gps", "Apply real-time GPS information from gpsd as EXIF tags (requires "LIBGPS_SO_VERSION")", 0},
};

static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]);
Expand Down Expand Up @@ -371,6 +394,7 @@ static void default_status(RASPISTILL_STATE *state)
state->datetime = 0;
state->timestamp = 0;
state->restart_interval = 0;
state->gpsdExif = 0;

// Setup preview window defaults
raspipreview_set_defaults(&state->preview_parameters);
Expand Down Expand Up @@ -763,6 +787,11 @@ static int parse_cmdline(int argc, const char **argv, RASPISTILL_STATE *state)
break;
}

case CommandGpsdExif:
state->gpsdExif = 1;
break;


default:
{
// Try parsing for any image specific parameters
Expand Down Expand Up @@ -1402,7 +1431,7 @@ static MMAL_STATUS_T add_exif_tag(RASPISTILL_STATE *state, const char *exif_tag)
* @param state Pointer to state control struct
*
*/
static void add_exif_tags(RASPISTILL_STATE *state)
static void add_exif_tags(RASPISTILL_STATE *state, struct gps_data_t *gpsdata)
{
time_t rawtime;
struct tm *timeinfo;
Expand Down Expand Up @@ -1437,6 +1466,127 @@ static void add_exif_tags(RASPISTILL_STATE *state)
snprintf(exif_buf, sizeof(exif_buf), "IFD0.DateTime=%s", time_buf);
add_exif_tag(state, exif_buf);


// Add GPS tags
if (state->gpsdExif)
{
// clear all existing tags first
add_exif_tag(state, "GPS.GPSDateStamp=");
add_exif_tag(state, "GPS.GPSTimeStamp=");
add_exif_tag(state, "GPS.GPSMeasureMode=");
add_exif_tag(state, "GPS.GPSSatellites=");
add_exif_tag(state, "GPS.GPSLatitude=");
add_exif_tag(state, "GPS.GPSLatitudeRef=");
add_exif_tag(state, "GPS.GPSLongitude=");
add_exif_tag(state, "GPS.GPSLongitudeRef=");
add_exif_tag(state, "GPS.GPSAltitude=");
add_exif_tag(state, "GPS.GPSAltitudeRef=");
add_exif_tag(state, "GPS.GPSSpeed=");
add_exif_tag(state, "GPS.GPSSpeedRef=");
add_exif_tag(state, "GPS.GPSTrack=");
add_exif_tag(state, "GPS.GPSTrackRef=");

pthread_mutex_lock(&gps_reader_data.gps_cache_mutex);
if (gpsdata->online)
{
if (state->verbose)
fprintf(stderr, "Adding GPS EXIF\n");
if (gpsdata->set & TIME_SET)
{
rawtime = gpsdata->fix.time;
timeinfo = localtime(&rawtime);
strftime(time_buf, sizeof(time_buf), "%Y:%m:%d", timeinfo);
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSDateStamp=%s", time_buf);
add_exif_tag(state, exif_buf);
strftime(time_buf, sizeof(time_buf), "%H/1,%M/1,%S/1", timeinfo);
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSTimeStamp=%s", time_buf);
add_exif_tag(state, exif_buf);
}
if (gpsdata->fix.mode >= MODE_2D)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSMeasureMode=%c",
(gpsdata->fix.mode >= MODE_3D) ? '3' : '2');
add_exif_tag(state, exif_buf);
if ((gpsdata->satellites_used > 0) && (gpsdata->satellites_visible > 0))
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Used:%d,Visible:%d",
gpsdata->satellites_used, gpsdata->satellites_visible);
add_exif_tag(state, exif_buf);
}
else if (gpsdata->satellites_used > 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Used:%d",
gpsdata->satellites_used);
add_exif_tag(state, exif_buf);
}
else if (gpsdata->satellites_visible > 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Visible:%d",
gpsdata->satellites_visible);
add_exif_tag(state, exif_buf);
}


if (gpsdata->set & LATLON_SET)
{
if (isnan(gpsdata->fix.latitude) == 0)
{
if (deg_to_str(fabs(gpsdata->fix.latitude), time_buf, sizeof(time_buf)) == 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLatitude=%s", time_buf);
add_exif_tag(state, exif_buf);
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLatitudeRef=%c",
(gpsdata->fix.latitude < 0) ? 'S' : 'N');
add_exif_tag(state, exif_buf);
}
}
if (isnan(gpsdata->fix.longitude) == 0)
{
if (deg_to_str(fabs(gpsdata->fix.longitude), time_buf, sizeof(time_buf)) == 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLongitude=%s", time_buf);
add_exif_tag(state, exif_buf);
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLongitudeRef=%c",
(gpsdata->fix.longitude < 0) ? 'W' : 'E');
add_exif_tag(state, exif_buf);
}
}
}
if ((gpsdata->set & ALTITUDE_SET) && (gpsdata->fix.mode >= MODE_3D))
{
if (isnan(gpsdata->fix.altitude) == 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSAltitude=%d/10",
(int)(gpsdata->fix.altitude*10+0.5));
add_exif_tag(state, exif_buf);
add_exif_tag(state, "GPS.GPSAltitudeRef=0");
}
}
if (gpsdata->set & SPEED_SET)
{
if (isnan(gpsdata->fix.speed) == 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSpeed=%d/10",
(int)(gpsdata->fix.speed*MPS_TO_KPH*10+0.5));
add_exif_tag(state, exif_buf);
add_exif_tag(state, "GPS.GPSSpeedRef=K");
}
}
if (gpsdata->set & TRACK_SET)
{
if (isnan(gpsdata->fix.track) == 0)
{
snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSTrack=%d/100",
(int)(gpsdata->fix.track*100+0.5));
add_exif_tag(state, exif_buf);
add_exif_tag(state, "GPS.GPSTrackRef=T");
}
}
}
}
pthread_mutex_unlock(&gps_reader_data.gps_cache_mutex);
}

// Now send any user supplied tags

for (i=0;i<state->numExifTags && i < MAX_USER_EXIF_TAGS;i++)
Expand Down Expand Up @@ -1771,6 +1921,58 @@ static void rename_file(RASPISTILL_STATE *state, FILE *output_file,
}
}

void *gps_reader_process(void *gps_reader_data_ptr)
{
GPS_READER_DATA *gps_reader = (GPS_READER_DATA *)gps_reader_data_ptr;
while (!gps_reader->terminated)
{
int ret = 0;
gps_reader->gpsd.gpsdata.set = 0;
gps_reader->gpsd.gpsdata.fix.mode = 0;
if ((connect_gpsd(&gps_reader->gpsd) < 0) ||
((ret = read_gps_data_once(&gps_reader->gpsd)) < 0))
break;

int gps_valid = 0;
if ((ret > 0) && (gps_reader->gpsd.gpsdata.online))
{
if (gps_reader->gpsd.gpsdata.fix.mode >= MODE_2D)
{
// we have GPS fix, copy fresh data to cache
gps_valid = 1;
time(&gps_reader->last_valid_time);
pthread_mutex_lock(&gps_reader->gps_cache_mutex);
memcpy(&gps_reader->gpsdata_cache, &gps_reader->gpsd.gpsdata,
sizeof(struct gps_data_t));
pthread_mutex_unlock(&gps_reader->gps_cache_mutex);
}
}
if (!gps_valid)
{
time_t now;
time(&now);
if (now - gps_reader->last_valid_time > GPS_CACHE_EXPIRY)
{
// our cache is stale, clear it
pthread_mutex_lock(&gps_reader->gps_cache_mutex);
gps_reader->gpsdata_cache.online = gps_reader->gpsd.gpsdata.online;
gps_reader->gpsdata_cache.set = 0;
gps_reader->gpsdata_cache.fix.mode = 0;
pthread_mutex_unlock(&gps_reader->gps_cache_mutex);
}
// we lost GPS fix, copy GPS time to cache if available
if (gps_reader->gpsd.gpsdata.set & TIME_SET)
{
pthread_mutex_lock(&gps_reader->gps_cache_mutex);
gps_reader->gpsdata_cache.set |= TIME_SET;
gps_reader->gpsdata_cache.fix.time = gps_reader->gpsd.gpsdata.fix.time;
pthread_mutex_unlock(&gps_reader->gps_cache_mutex);
}
}
}
return NULL;
}

/**
* main
*/
Expand Down Expand Up @@ -1826,6 +2028,49 @@ int main(int argc, const char **argv)
dump_status(&state);
}

if (state.gpsdExif)
{
memset(&gps_reader_data, 0, sizeof(gps_reader_data));
pthread_mutex_init(&gps_reader_data.gps_cache_mutex, NULL);
gps_reader_data.pstate = &state;

gpsd_init(&gps_reader_data.gpsd);
if (libgps_load(&gps_reader_data.gpsd))
{
pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
exit(EX_SOFTWARE);
}
if (state.verbose)
fprintf(stderr, "Connecting to gpsd @ %s:%s\n",
gps_reader_data.gpsd.server, gps_reader_data.gpsd.port);
if (connect_gpsd(&gps_reader_data.gpsd))
{
fprintf(stderr, "no gpsd running or network error: %d, %s\n",
errno, gps_reader_data.gpsd.gps_errstr(errno));
libgps_unload(&gps_reader_data.gpsd);
pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
exit(EX_SOFTWARE);
}
if (state.verbose)
fprintf(stderr, "Waiting for GPS time\n");
if (wait_gps_time(&gps_reader_data.gpsd, 2))
{
if (state.verbose)
fprintf(stderr, "Warning: GPS time not available\n");
}
if (state.verbose)
fprintf(stderr, "Creating GPS reader thread\n");
if (pthread_create(&gps_reader_data.gps_reader_thread, NULL,
gps_reader_process, &gps_reader_data))
{
fprintf(stderr, "Error creating GPS reader thread\n");
exit_code = EX_SOFTWARE;
gps_reader_data.terminated = 1;
goto gps_error;
}
gps_reader_data.gps_reader_thread_ok = 1;
}

if (state.useGL)
raspitex_init(&state.raspitex_state);

Expand Down Expand Up @@ -2009,7 +2254,7 @@ int main(int argc, const char **argv)
// once enabled no further exif data is accepted
if ( state.enableExifTags )
{
add_exif_tags(&state);
add_exif_tags(&state, &gps_reader_data.gpsdata_cache);
}
else
{
Expand Down Expand Up @@ -2166,6 +2411,23 @@ int main(int argc, const char **argv)
fprintf(stderr, "Close down completed, all components disconnected, disabled and destroyed\n\n");
}

gps_error:
if (state.gpsdExif)
{
gps_reader_data.terminated = 1;
if (gps_reader_data.gps_reader_thread_ok)
{
if (state.verbose)
fprintf(stderr, "Waiting for GPS reader thread to terminate\n");
pthread_join(gps_reader_data.gps_reader_thread, NULL);
}
if ((state.verbose) && (gps_reader_data.gpsd.gpsd_connected))
fprintf(stderr, "Closing gpsd connection\n\n");
disconnect_gpsd(&gps_reader_data.gpsd);
libgps_unload(&gps_reader_data.gpsd);
pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
}

if (status != MMAL_SUCCESS)
raspicamcontrol_check_configuration(128);

Expand Down
Loading

0 comments on commit 4228b6c

Please sign in to comment.