mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
Digital cam pwm trigger (#3636)
* [gps] send NMEA frames to external sensors * [digital_cam] add pwm trigger module
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
eaeee4879a
commit
879c971eb0
@@ -0,0 +1,58 @@
|
||||
<!DOCTYPE module SYSTEM "./module.dtd">
|
||||
|
||||
<module name="digital_cam_pwm" dir="digital_cam">
|
||||
<doc>
|
||||
<description>
|
||||
Trigger using pwm cameras like MAPIR (https://www.mapir.camera/en-gb)
|
||||
or ThermalCapture 2.0 from TeAx (https://thermalcapture.com/thermalcapture-2-0/)
|
||||
|
||||
**** MAPIR ****
|
||||
Duty cycle == 2000us : TRIGGER_ON
|
||||
Duty cycle == 1500us : SD Unmount
|
||||
Duty cycle == 1000us : TRIGGER_OFF
|
||||
Image capture frequency : 1.5s for JPG and 2.5-3.0s RAW+JPG
|
||||
|
||||
The quickest the 2000us command should be sent is about once every 1.5s as the camera cannot
|
||||
capture JPG images more quickly than 1.5s. For RAW+JPG mode we recommend a 2.5-3.0s wait time.
|
||||
|
||||
RAW images are used for capturing data for reflectance measurements, otherwise the pixels in the JPG are not usable.
|
||||
im_freq_timer should therefore be superior 1.5s (TRIGGER_CAMERA_CAPTURE_IMAGE_PERIOD = 1.5 seconds)
|
||||
|
||||
NMEA GPS data are stored in metadata
|
||||
|
||||
**** ThermalCapture ****
|
||||
Duty cycle inferior 1500us : TRIGGER_OFF
|
||||
Duty cycle superior 1500us : TRIGGER_ON
|
||||
Image capture frequency up to 30 Hz
|
||||
Possibility to capture one frame per trigger in configurator : Trigger frame mode
|
||||
Convert 14-bit data into temperature values :
|
||||
High gain mode: temp [°C] = raw * 0.04 - 273.15
|
||||
Low gain mode : temp [°C] = raw * 0.4 - 273.15
|
||||
|
||||
NMEA GPS data are stored in metadata
|
||||
|
||||
</description>
|
||||
<section name="DC_CAM_PWM">
|
||||
<define name="DC_CAM_PWM_SHUTTER_DELAY" description="how long to push shutter in seconds"/>
|
||||
<define name="DC_CAM_PWM_SERVO" value="DC_CAM_TRIGGER" description="name of the servo to trigger with PWM"/>
|
||||
</section>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>digital_cam_common</depends>
|
||||
<conflicts>digital_cam_gpio,digital_cam_i2c,digital_cam_uart,digital_cam_video,digital_cam_pprzlink,digital_cam_servo</conflicts>
|
||||
<recommends>gps_nmea_send</recommends>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="dc_shoot_pwm.h"/>
|
||||
</header>
|
||||
<init fun="dc_shoot_pwm_init()"/>
|
||||
<periodic fun="dc_shoot_pwm_periodic()" freq="20" autorun="TRUE"/>
|
||||
<makefile>
|
||||
<file name="dc_shoot_pwm.c"/>
|
||||
<test arch="chibios">
|
||||
<define name="ACTUATORS_NB" value="1"/>
|
||||
<define name="SERVO_DC_CAM_TRIGGER_IDX" value="0"/>
|
||||
<define name="DC_SHOOT_PWM_PERIODIC_FREQ" value="20"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,36 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="gps_nmea_send" dir="gps">
|
||||
<doc>
|
||||
<description>
|
||||
module used to send GPS data over for external instrument using NMEA input.
|
||||
Example: MAPIR camera stores GPS data in metadata on each frame.
|
||||
</description>
|
||||
<configure name="NMEA_SEND_UART" value="UART3" description="UART on which NMEA frames are sent"/>
|
||||
<configure name="NMEA_SEND_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
|
||||
<define name="NMEA_SEND_USE_STATE_DATA" value="FALSE|TRUE" description="Use filtered state data rather than raw GPS data (default: FALSE)"/>
|
||||
</doc>
|
||||
<dep>
|
||||
<depends>uart</depends>
|
||||
</dep>
|
||||
<header>
|
||||
<file name="gps_nmea_send.h"/>
|
||||
</header>
|
||||
<init fun="gps_nmea_send_init()"/>
|
||||
<periodic fun="gps_nmea_send_periodic()" freq="5" autorun="TRUE"/>
|
||||
<makefile target="ap">
|
||||
<configure name="NMEA_SEND_UART" default="uart3" case="upper|lower"/>
|
||||
<configure name="NMEA_SEND_BAUD" default="B115200"/>
|
||||
<file name="gps_nmea_send.c"/>
|
||||
<define name="USE_$(NMEA_SEND_UART_UPPER)"/>
|
||||
<define name="NMEA_SEND_UART" value="$(NMEA_SEND_UART_LOWER)"/>
|
||||
<define name="$(NMEA_SEND_UART_UPPER)_BAUD" value="$(NMEA_SEND_BAUD)"/>
|
||||
<test>
|
||||
<define name="USE_UART1"/>
|
||||
<define name="UART1_BAUD" value="B115200"/>
|
||||
<define name="NMEA_SEND_UART" value="uart1"/>
|
||||
</test>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* Copyright (C) 2025 Jean-Baptiste Forestier <jean-baptiste.forestier@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/digital_cam/dc_shoot_pwm.c"
|
||||
* @author Jean-Baptiste Forestier
|
||||
* Trigger using pwm cameras like MAPIR (https://www.mapir.camera/en-gb)
|
||||
* or ThermalCapture 2.0 from TeAx (https://thermalcapture.com/thermalcapture-2-0/)
|
||||
*
|
||||
* **** MAPIR ****
|
||||
* Duty cycle == 2000us => TRIGGER_ON
|
||||
* Duty cycle == 1500us => SD Unmount
|
||||
* Duty cycle == 1000us => TRIGGER_OFF
|
||||
* Image capture frequency = 1.5s for JPG and 2.5-3.0s RAW+JPG
|
||||
*
|
||||
* The quickest the 2000us command should be sent is about once every 1.5s as the camera cannot
|
||||
* capture JPG images more quickly than 1.5s. For RAW+JPG mode we recommend a 2.5-3.0s wait time.
|
||||
*
|
||||
* RAW images are used for capturing data for reflectance measurements, otherwise the pixels in the JPG are not usable.
|
||||
* im_freq_timer should therefore be > 1.5s (=>TRIGGER_CAMERA_CAPTURE_IMAGE_PERIOD = 1.5 seconds)
|
||||
*
|
||||
* NMEA GPS data are stored in metadata
|
||||
*
|
||||
* **** ThermalCapture ****
|
||||
* Duty cycle < 1500us => TRIGGER_OFF
|
||||
* Duty cycle > 1500us => TRIGGER_ON
|
||||
* Image capture frequency up to 30 Hz
|
||||
* Possibility to capture one frame per trigger in configurator : Trigger frame mode
|
||||
* Convert 14-bit data into temperature values :
|
||||
* High gain mode: temp [°C] = raw * 0.04 - 273.15
|
||||
* Low gain mode : temp [°C] = raw * 0.4 - 273.15
|
||||
*
|
||||
* NMEA GPS data are stored in metadata
|
||||
*
|
||||
* **** Trigger system ****
|
||||
*
|
||||
* TIME ────────────────────────────────────────────────────────────>
|
||||
*
|
||||
* PWM 1000us 2000us 1000us 2000us
|
||||
* Vcc ┌────┐ ┌───────┐ ┌────┐ ┌───────┐
|
||||
* │ OFF│ │ ON │ │ OFF│ │ ON │
|
||||
* 0V ────┘ └────────┘ └───────┘ └────────┘ └────
|
||||
* im_freq_timer: ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 0 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 0 ↓↓↓↓↓↓↓↓↓↓
|
||||
* shutter_timer: ↓↓↓↓ 0 ↓↓↓↓ 0
|
||||
*
|
||||
* ↑ ↑
|
||||
* Picture 1 Picture 2
|
||||
*
|
||||
*/
|
||||
|
||||
#include "modules/digital_cam/dc_shoot_pwm.h"
|
||||
|
||||
// Include Standard Camera Control Interface
|
||||
#include "dc.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "generated/modules.h"
|
||||
#include "modules/actuators/actuators.h"
|
||||
|
||||
|
||||
/** how long to push shutter in seconds */
|
||||
#ifndef DC_CAM_PWM_SHUTTER_DELAY
|
||||
#define DC_CAM_PWM_SHUTTER_DELAY 0.1
|
||||
#endif
|
||||
|
||||
/** Max PWM Value */
|
||||
#ifndef DC_CAM_PWM_ON_VALUE
|
||||
#define DC_CAM_PWM_ON_VALUE MAX_PPRZ
|
||||
#endif
|
||||
|
||||
/** Min PWM Value */
|
||||
#ifndef DC_CAM_PWM_OFF_VALUE
|
||||
#define DC_CAM_PWM_OFF_VALUE MIN_PPRZ
|
||||
#endif
|
||||
|
||||
/** Servo destination */
|
||||
#ifndef DC_CAM_PWM_SERVO
|
||||
#define DC_CAM_PWM_SERVO DC_CAM_TRIGGER
|
||||
#endif
|
||||
|
||||
#define CamActuatorSet(_a, _v) ActuatorSet(_a, _v)
|
||||
|
||||
/**
|
||||
* Timer used for Shutter delay control
|
||||
*/
|
||||
static uint8_t shutter_timer;
|
||||
|
||||
/**
|
||||
* Initialization function
|
||||
*/
|
||||
void dc_shoot_pwm_init(void)
|
||||
{
|
||||
shutter_timer = 0;
|
||||
CamActuatorSet(DC_CAM_PWM_SERVO, DC_CAM_PWM_OFF_VALUE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Periodic function to send data
|
||||
*/
|
||||
void dc_shoot_pwm_periodic(void)
|
||||
{
|
||||
// Manage the shutter opening time each DC_CAM_PWM_SHUTTER_DELAY seconds
|
||||
if (shutter_timer == 0) {
|
||||
CamActuatorSet(DC_CAM_PWM_SERVO, DC_CAM_PWM_OFF_VALUE);
|
||||
} else {
|
||||
shutter_timer--;
|
||||
}
|
||||
|
||||
// Common DC Periodic task
|
||||
dc_periodic();
|
||||
}
|
||||
|
||||
/* Command The Camera */
|
||||
void dc_send_command(uint8_t cmd)
|
||||
{
|
||||
if (cmd == DC_SHOOT) {
|
||||
CamActuatorSet(DC_CAM_PWM_SERVO, DC_CAM_PWM_ON_VALUE);
|
||||
shutter_timer = DC_CAM_PWM_SHUTTER_DELAY * DC_SHOOT_PWM_PERIODIC_FREQ;
|
||||
dc_send_shot_position();
|
||||
}
|
||||
|
||||
// call command send_command function
|
||||
dc_send_command_common(cmd);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Copyright (C) 2025 Jean-Baptiste Forestier <jean-baptiste.forestier@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/digital_cam/dc_shoot_pwm.h
|
||||
* @author Jean-Baptiste Forestier
|
||||
* Trigger using pwm cameras like MAPIR (https://www.mapir.camera/en-gb)
|
||||
* or ThermalCapture 2.0 from TeAx (https://thermalcapture.com/thermalcapture-2-0/)
|
||||
*
|
||||
* **** MAPIR ****
|
||||
* Duty cycle == 2000us => TRIGGER_ON
|
||||
* Duty cycle == 1500us => SD Unmount
|
||||
* Duty cycle == 1000us => TRIGGER_OFF
|
||||
* Image capture frequency = 1.5s for JPG and 2.5-3.0s RAW+JPG
|
||||
*
|
||||
* The quickest the 2000us command should be sent is about once every 1.5s as the camera cannot
|
||||
* capture JPG images more quickly than 1.5s. For RAW+JPG mode we recommend a 2.5-3.0s wait time.
|
||||
*
|
||||
* RAW images are used for capturing data for reflectance measurements, otherwise the pixels in the JPG are not usable.
|
||||
* im_freq_timer should therefore be > 1.5s (=>TRIGGER_CAMERA_CAPTURE_IMAGE_PERIOD = 1.5 seconds)
|
||||
*
|
||||
* NMEA GPS data are stored in metadata
|
||||
*
|
||||
* **** ThermalCapture ****
|
||||
* Duty cycle < 1500us => TRIGGER_OFF
|
||||
* Duty cycle > 1500us => TRIGGER_ON
|
||||
* Image capture frequency up to 30 Hz
|
||||
* Possibility to capture one frame per trigger in configurator : Trigger frame mode
|
||||
* Convert 14-bit data into temperature values :
|
||||
* High gain mode: temp [°C] = raw * 0.04 - 273.15
|
||||
* Low gain mode : temp [°C] = raw * 0.4 - 273.15
|
||||
*
|
||||
* NMEA GPS data are stored in metadata
|
||||
*
|
||||
* **** Trigger system ****
|
||||
*
|
||||
* TIME ────────────────────────────────────────────────────────────>
|
||||
*
|
||||
* PWM 1000us 2000us 1000us 2000us
|
||||
* Vcc ┌────┐ ┌───────┐ ┌────┐ ┌───────┐
|
||||
* │ OFF│ │ ON │ │ OFF│ │ ON │
|
||||
* 0V ────┘ └────────┘ └───────┘ └────────┘ └────
|
||||
* im_freq_timer: ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 0 ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ 0 ↓↓↓↓↓↓↓↓↓↓
|
||||
* shutter_timer: ↓↓↓↓ 0 ↓↓↓↓ 0
|
||||
*
|
||||
* ↑ ↑
|
||||
* Picture 1 Picture 2
|
||||
*
|
||||
* im_freq_timer should therefore be > 1.5s (=>TRIGGER_CAMERA_CAPTURE_IMAGE_PERIOD = 1.5 seconds)
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef DC_SHOOT_PWM_H
|
||||
#define DC_SHOOT_PWM_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
void dc_shoot_pwm_init(void);
|
||||
void dc_shoot_pwm_periodic(void);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,277 @@
|
||||
/*
|
||||
* Copyright (C) 2025 Jean-Baptiste FORESTIER <jean-baptiste.forestier@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/gps/gps_nmea_send.c"
|
||||
* @author Jean-Baptiste FORESTIER
|
||||
* @brief module used to send GPS data over a Tawaki UART for extern instrument using NMEA protocol
|
||||
* Exemple of use : MAPIR camera stores GPS data in metadata on each frame
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "modules/gps/gps_nmea_send.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h" // State interface for rotation compensation
|
||||
#include "modules/gps/gps.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
// ** Declaration ** //
|
||||
struct Gps_Nmea_Send gps_nmea_send;
|
||||
|
||||
static void recover_gps_data(void);
|
||||
static void build_nmea_sentence(void);
|
||||
static uint8_t nmea_checksum(const char *sentence, int length);
|
||||
static void nmea_convert_deg_to_DDMM(double deg, char *buf, int is_lat);
|
||||
static void gps_nmea_get_system_date_str(char *buf_date, size_t buf_date_size, char *buf_time, size_t buf_time_size);
|
||||
static void nmea_send(const char *payload, int payload_length);
|
||||
|
||||
|
||||
/**
|
||||
* Initialization function
|
||||
*/
|
||||
void gps_nmea_send_init(void)
|
||||
{
|
||||
gps_nmea_send.error_init = false;
|
||||
|
||||
gps_nmea_send.msg.lat = 0;
|
||||
gps_nmea_send.msg.lon = 0;
|
||||
gps_nmea_send.msg.num_sv = 0;
|
||||
gps_nmea_send.msg.pdop = 0;
|
||||
gps_nmea_send.msg.hmsl = 0;
|
||||
gps_nmea_send.msg.vground = 0;
|
||||
gps_nmea_send.msg.course = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Periodic function to send data
|
||||
*/
|
||||
void gps_nmea_send_periodic(void)
|
||||
{
|
||||
recover_gps_data();
|
||||
build_nmea_sentence();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Function to recover data available onboard from GPS
|
||||
*/
|
||||
void recover_gps_data(void)
|
||||
{
|
||||
#if GPS_NMEA_SEND_USE_STATE_DATA
|
||||
struct LlaCoor_f *lla = stateGetPositionLla_f();
|
||||
gps_nmea_send.msg.lat = (double)gps->lat;
|
||||
gps_nmea_send.msg.lon = (double)gps->lon;
|
||||
gps_nmea_send.msg.hmsl = stateGetHmslOrigin_f() + stateGetPositionEnu_f()->z;
|
||||
gps_nmea_send.msg.vground = stateGetHorizontalSpeedNorm_f();
|
||||
gps_nmea_send.msg.course = NormCourseRad(stateGetHorizontalSpeedDir_f());
|
||||
#else // Default, use unfiltered GPS data
|
||||
gps_nmea_send.msg.lat = (double)gps.lla_pos.lat / 1e7;
|
||||
gps_nmea_send.msg.lon = (double)gps.lla_pos.lon / 1e7;
|
||||
gps_nmea_send.msg.hmsl = (float)gps.hmsl / 1000.f; //in meters
|
||||
gps_nmea_send.msg.vground = (float)gps.gspeed / 100.f; // in meters
|
||||
gps_nmea_send.msg.course = (float)DegOfRad(gps.course / 1e7);
|
||||
#endif
|
||||
gps_nmea_send.msg.num_sv = gps.num_sv;
|
||||
gps_nmea_send.msg.pdop = gps.pdop;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to build sentence for NMEA Protocol
|
||||
*/
|
||||
void build_nmea_sentence(void)
|
||||
{
|
||||
// Latitude and Longitude
|
||||
char lat_buf[16], lon_buf[16], gga[120], rmc[120], date_sys[6], time_sys[6];
|
||||
int len_gga = 0;
|
||||
int len_rmc = 0;
|
||||
uint8_t cs;
|
||||
nmea_convert_deg_to_DDMM(gps_nmea_send.msg.lat, lat_buf, 1);
|
||||
nmea_convert_deg_to_DDMM(gps_nmea_send.msg.lon, lon_buf, 0);
|
||||
char lat_hemi = (gps_nmea_send.msg.lat >= 0) ? 'N' : 'S';
|
||||
char lon_hemi = (gps_nmea_send.msg.lon >= 0) ? 'E' : 'W';
|
||||
|
||||
for (int i = 0; i < 120; i++) { gga[i] = '\0'; rmc[i] = '\0'; }
|
||||
|
||||
if (gps.fix >= GPS_FIX_3D) {
|
||||
gps_nmea_get_system_date_str(date_sys, sizeof(date_sys) + 1, time_sys, sizeof(time_sys) + 1);
|
||||
|
||||
// -------------------------------
|
||||
// GPGGA Frame
|
||||
// -------------------------------
|
||||
sprintf(gga,
|
||||
"$GPGGA,%s,%s,%c,%s,%c,1,%02u,%04u,%09.3f,M,0,M,,",
|
||||
time_sys, lat_buf, lat_hemi, lon_buf, lon_hemi,
|
||||
gps_nmea_send.msg.num_sv, gps_nmea_send.msg.pdop,
|
||||
gps_nmea_send.msg.hmsl);
|
||||
len_gga = 7; // $GPGGA, = 7
|
||||
len_gga += 7; // time_nmea, = 7
|
||||
len_gga += 10; // lat_buf = 9 + comma
|
||||
len_gga += 2; // lat_hemi = N,
|
||||
len_gga += 11; // lon_buf = 10 + comma
|
||||
len_gga += 2; // lon_hemi = W,
|
||||
len_gga += 2; // GPS Quality indicator GPS fix = 1,
|
||||
len_gga += 3; // number of sats = 08,
|
||||
len_gga += 5; // position dilution of precision scaled by 100 = 1020,
|
||||
len_gga += 10; // height above mean sea level (MSL) in m = 02945.127,
|
||||
len_gga += 2; // height above mean sea level unit = M,
|
||||
len_gga += 2; // Ellipsoid to geoid distance set 0, don't where to find it = 0,
|
||||
len_gga += 2; // Distance above unit = M,
|
||||
len_gga += 1; // Empty = ,
|
||||
|
||||
cs = nmea_checksum(gga, len_gga);
|
||||
sprintf(gga + len_gga, "*%02X\r\n", cs);
|
||||
len_gga += 5; // * + Checksum + \r\n = *4F\r\n
|
||||
|
||||
nmea_send(gga, len_gga);
|
||||
|
||||
// -------------------------------
|
||||
// GPRMC Frame
|
||||
// -------------------------------
|
||||
sprintf(rmc,
|
||||
"$GPRMC,%s,A,%s,%c,%s,%c,%05.1f,%05.1f,%s,000.0,W",
|
||||
time_sys, lat_buf, lat_hemi, lon_buf, lon_hemi,
|
||||
gps_nmea_send.msg.vground, gps_nmea_send.msg.course, date_sys);
|
||||
len_rmc = 7; // $GPRMC, = 7
|
||||
len_rmc += 7; // time_nmea, = 7
|
||||
len_rmc += 2; // Status A=active or V=void = A,
|
||||
len_rmc += 10; // lat_buf = 9 + comma
|
||||
len_rmc += 2; // lat_hemi = N,
|
||||
len_rmc += 11; // lon_buf = 10 + comma
|
||||
len_rmc += 2; // lon_hemi = W,
|
||||
len_rmc += 6; // vground = 050.4,
|
||||
len_rmc += 6; // track angle course = 050.4,
|
||||
len_rmc += 7; // Date = 230394,
|
||||
len_rmc += 7; // Magnetic variation, in degrees = 003.1,W,
|
||||
|
||||
cs = nmea_checksum(rmc, len_rmc);
|
||||
sprintf(rmc + len_rmc, "*%02X\r\n", cs);
|
||||
len_rmc += 5; // * + Checksum + \r\n = *4F\r\n
|
||||
|
||||
nmea_send(rmc, len_rmc);
|
||||
|
||||
} else {
|
||||
//If no fix, empty GGA
|
||||
sprintf(gga, "$GPGGA,,,,,,0,00,99.99,,,,,,*68\r\n");
|
||||
nmea_send(gga, 33);
|
||||
|
||||
//If no fix, empty RMC
|
||||
sprintf(rmc, "$GPRMC,,V,,,,,,,,,,*53\r\n");
|
||||
nmea_send(rmc, 24);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to calculate checksum
|
||||
*/
|
||||
uint8_t nmea_checksum(const char *sentence, int length)
|
||||
{
|
||||
uint8_t cs = 0;
|
||||
for (int i = 0; i < length; i++) {
|
||||
cs ^= (uint8_t)sentence[i];
|
||||
}
|
||||
return cs;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to convert lat long in DDLL.MMMM Format
|
||||
* Lat and long don't have the same length
|
||||
*/
|
||||
void nmea_convert_deg_to_DDMM(double deg, char *buf, int is_lat)
|
||||
{
|
||||
double abs_deg = fabs(deg);
|
||||
int d = (int)abs_deg;
|
||||
double minutes = (abs_deg - d) * 60.0;
|
||||
|
||||
if (is_lat) {
|
||||
// 2 integers for degrees 0 to 90 without sign => %02d | 2 integers for minutes, 1 for point, 4 for minutes decimals
|
||||
sprintf(buf, "%02d%07.4f", d, minutes);
|
||||
} else {
|
||||
// 3 integers for degrees 0 to 180 without sign => %03d | 2 integers for minutes, 1 for point, 4 for minutes decimals
|
||||
sprintf(buf, "%03d%07.4f", d, minutes);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Date from system
|
||||
*/
|
||||
void gps_nmea_get_system_date_str(char *buf_date, size_t buf_date_size, char *buf_time, size_t buf_time_size)
|
||||
{
|
||||
// days from 1970-01-01
|
||||
const int64_t gps_epoch_days = 3657;
|
||||
// Total time gps
|
||||
uint64_t gps_sec = (uint64_t)gps.week * 7 * 24 * 3600 + gps.tow / 1000;
|
||||
// Day gps + gps epoch
|
||||
int64_t days = gps_epoch_days + gps_sec / 86400;
|
||||
|
||||
// Algo to get year month day from Week and tow
|
||||
// Shift day count so that day 0 corresponds to the Gregorian epoch (0000-03-01)
|
||||
days += 719468;
|
||||
// Determine the 400-year Gregorian cycle, One Gregorian era = 400 years = 146097 days.
|
||||
int64_t era = (days >= 0 ? days : days - 146096) / 146097;
|
||||
// Day Of Era: number of days elapsed within the current 400-year cycle.
|
||||
uint64_t doe = (uint64_t)(days - era * 146097);
|
||||
// Year Of Era: compute the year number inside the cycle, correcting for leap years (every 4 years, except centuries, except 400-year multiples).
|
||||
uint64_t yoe = (doe - doe / 1460 + doe / 36524 - doe / 146096) / 365;
|
||||
// Convert "year in era" to an absolute Gregorian year.
|
||||
int64_t y = (int64_t)yoe + era * 400;
|
||||
// Day Of Year: subtract all full days from completed years to get day index within the year.
|
||||
uint64_t doy = doe - (365 * yoe + yoe / 4 - yoe / 100);
|
||||
// Month part: day-of-year into a month index in a calendar where March = 0 and February = 11
|
||||
uint64_t mp = (5 * doy + 2) / 153;
|
||||
// Compute the day-of-month
|
||||
uint64_t d = doy - (153 * mp + 2) / 5 + 1;
|
||||
// Convert shifted month index back to standard month 0-12
|
||||
uint64_t m = mp + (mp < 10 ? 3 : -9);
|
||||
y += (m <= 2);
|
||||
|
||||
// Hours / minutes / secondes
|
||||
uint32_t sec_of_day = gps_sec % 86400;
|
||||
uint8_t hour = sec_of_day / 3600;
|
||||
uint8_t min = (sec_of_day % 3600) / 60;
|
||||
uint8_t sec = sec_of_day % 60;
|
||||
|
||||
// Format : DDMMYY
|
||||
snprintf(buf_date, buf_date_size, "%02d%02d%02d",
|
||||
(uint8_t)d,
|
||||
(uint8_t)m,
|
||||
(uint8_t)(y % 100));
|
||||
|
||||
// Format : HHMMSS
|
||||
snprintf(buf_time, buf_time_size, "%02d%02d%02d",
|
||||
hour,
|
||||
min,
|
||||
sec);
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to send payload to UART
|
||||
*/
|
||||
void nmea_send(const char *payload, int payload_length)
|
||||
{
|
||||
uart_put_buffer(&NMEA_SEND_UART, 0, (const uint8_t *)payload, payload_length);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* Copyright (C) 2025 Jean-Baptiste FORESTIER <jean-baptiste.forestier@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/gps/gps_nmea_send.h"
|
||||
* @author Jean-Baptiste FORESTIER
|
||||
* @brief module used to send GPS data over a Tawaki UART for extern instrument using NMEA protocol
|
||||
* Exemple of use : MAPIR camera stores GPS data in metadata on each frame
|
||||
*/
|
||||
|
||||
#ifndef GPS_NMEA_SEND_H
|
||||
#define GPS_NMEA_SEND_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
struct gps_nmea_send_msg_t {
|
||||
double lat; ///< Latitude
|
||||
double lon; ///< Longiitude
|
||||
uint8_t num_sv; ///< number of sat in fix
|
||||
uint16_t pdop; ///< position dilution of precision scaled by 100
|
||||
float hmsl; ///< Orthometric height (MSL reference)
|
||||
float vground; ///< Speed over ground in m/s
|
||||
float course; ///< GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
|
||||
};
|
||||
|
||||
struct Gps_Nmea_Send {
|
||||
bool error_init; // Flag to indicate if there was an error during initialization
|
||||
struct gps_nmea_send_msg_t msg;
|
||||
};
|
||||
|
||||
extern void gps_nmea_send_init(void);
|
||||
extern void gps_nmea_send_periodic(void);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user