forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
commit
17a1c986c2
|
|
@ -57,7 +57,7 @@ def main():
|
|||
for (root, dirs, files) in os.walk(args.folder):
|
||||
for file in files:
|
||||
# only prune text files
|
||||
if ".zip" in file or ".bin" in file or ".swp" in file:
|
||||
if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file:
|
||||
continue
|
||||
|
||||
file_path = os.path.join(root, file)
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@
|
|||
typedef enum {
|
||||
GPS_DRIVER_MODE_NONE = 0,
|
||||
GPS_DRIVER_MODE_UBX,
|
||||
GPS_DRIVER_MODE_MTK
|
||||
GPS_DRIVER_MODE_MTK,
|
||||
GPS_DRIVER_MODE_ASHTECH
|
||||
} gps_driver_mode_t;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -165,7 +165,7 @@ ORB_DECLARE(output_pwm);
|
|||
|
||||
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
|
||||
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
|
||||
|
||||
/** power up DSM receiver */
|
||||
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,641 @@
|
|||
#include "ashtech.h"
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/satellite_info.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
|
||||
ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
|
||||
_fd(fd),
|
||||
_satellite_info(satellite_info),
|
||||
_gps_position(gps_position)
|
||||
{
|
||||
decode_init();
|
||||
_decode_state = NME_DECODE_UNINIT;
|
||||
_rx_buffer_bytes = 0;
|
||||
}
|
||||
|
||||
ASHTECH::~ASHTECH()
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* All NMEA descriptions are taken from
|
||||
* http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
|
||||
*/
|
||||
|
||||
int ASHTECH::handle_message(int len)
|
||||
{
|
||||
char * endp;
|
||||
|
||||
if (len < 7) { return 0; }
|
||||
|
||||
int uiCalcComma = 0;
|
||||
|
||||
for (int i = 0 ; i < len; i++) {
|
||||
if (_rx_buffer[i] == ',') { uiCalcComma++; }
|
||||
}
|
||||
|
||||
char *bufptr = (char *)(_rx_buffer + 6);
|
||||
|
||||
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
|
||||
/*
|
||||
UTC day, month, and year, and local time zone offset
|
||||
An example of the ZDA message string is:
|
||||
|
||||
$GPZDA,172809.456,12,07,1996,00,00*45
|
||||
|
||||
ZDA message fields
|
||||
Field Meaning
|
||||
0 Message ID $GPZDA
|
||||
1 UTC
|
||||
2 Day, ranging between 01 and 31
|
||||
3 Month, ranging between 01 and 12
|
||||
4 Year
|
||||
5 Local time zone offset from GMT, ranging from 00 through 13 hours
|
||||
6 Local time zone offset from GMT, ranging from 00 through 59 minutes
|
||||
7 The checksum data, always begins with *
|
||||
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
|
||||
*/
|
||||
double ashtech_time = 0.0;
|
||||
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
|
||||
int ashtech_hour = ashtech_time / 10000;
|
||||
int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100;
|
||||
double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
|
||||
/*
|
||||
* convert to unix timestamp
|
||||
*/
|
||||
struct tm timeinfo;
|
||||
timeinfo.tm_year = year - 1900;
|
||||
timeinfo.tm_mon = month - 1;
|
||||
timeinfo.tm_mday = day;
|
||||
timeinfo.tm_hour = ashtech_hour;
|
||||
timeinfo.tm_min = ashtech_minute;
|
||||
timeinfo.tm_sec = int(ashtech_sec);
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
|
||||
_gps_position->timestamp_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) {
|
||||
/*
|
||||
Time, position, and fix related data
|
||||
An example of the GBS message string is:
|
||||
|
||||
$GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
|
||||
|
||||
Note - The data string exceeds the ASHTECH standard length.
|
||||
GGA message fields
|
||||
Field Meaning
|
||||
0 Message ID $GPGGA
|
||||
1 UTC of position fix
|
||||
2 Latitude
|
||||
3 Direction of latitude:
|
||||
N: North
|
||||
S: South
|
||||
4 Longitude
|
||||
5 Direction of longitude:
|
||||
E: East
|
||||
W: West
|
||||
6 GPS Quality indicator:
|
||||
0: Fix not valid
|
||||
1: GPS fix
|
||||
2: Differential GPS fix, OmniSTAR VBS
|
||||
4: Real-Time Kinematic, fixed integers
|
||||
5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
|
||||
7 Number of SVs in use, range from 00 through to 24+
|
||||
8 HDOP
|
||||
9 Orthometric height (MSL reference)
|
||||
10 M: unit of measure for orthometric height is meters
|
||||
11 Geoid separation
|
||||
12 M: geoid separation measured in meters
|
||||
13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
|
||||
14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
|
||||
15
|
||||
The checksum data, always begins with *
|
||||
Note - If a user-defined geoid model, or an inclined
|
||||
*/
|
||||
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
|
||||
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
|
||||
double hdop __attribute__((unused)) = 99.9;
|
||||
char ns = '?', ew = '?';
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (ns == 'S') {
|
||||
lat = -lat;
|
||||
}
|
||||
|
||||
if (ew == 'W') {
|
||||
lon = -lon;
|
||||
}
|
||||
|
||||
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||
_gps_position->alt = alt * 1000;
|
||||
_rate_count_lat_lon++;
|
||||
|
||||
if (fix_quality <= 0) {
|
||||
_gps_position->fix_type = 0;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
|
||||
* and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
|
||||
*/
|
||||
if (fix_quality == 5) { fix_quality = 3; }
|
||||
|
||||
/*
|
||||
* fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
|
||||
*/
|
||||
_gps_position->fix_type = 3 + fix_quality - 1;
|
||||
}
|
||||
|
||||
_gps_position->timestamp_position = hrt_absolute_time();
|
||||
|
||||
_gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */
|
||||
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
|
||||
_gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */
|
||||
_gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */
|
||||
_gps_position->cog_rad =
|
||||
0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
||||
_gps_position->c_variance_rad = 0.1;
|
||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
return 1;
|
||||
|
||||
} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
|
||||
/*
|
||||
Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
|
||||
|
||||
$PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc
|
||||
Parameter Description Range
|
||||
d1 Position mode 0: standalone
|
||||
1: differential
|
||||
2: RTK float
|
||||
3: RTK fixed
|
||||
5: Dead reckoning
|
||||
9: SBAS (see NPT setting)
|
||||
d2 Number of satellite used in position fix 0-99
|
||||
m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99
|
||||
m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes
|
||||
c5 Latitude sector N, S
|
||||
m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes
|
||||
c7 Longitude sector E,W
|
||||
f8 Altitude above ellipsoid +9999.000
|
||||
f9 Differential age (data link age), seconds 0.0-600.0
|
||||
f10 True track/course over ground in degrees 0.0-359.9
|
||||
f11 Speed over ground in knots 0.0-999.9
|
||||
f12 Vertical velocity in decimeters per second +999.9
|
||||
f13 PDOP 0-99.9
|
||||
f14 HDOP 0-99.9
|
||||
f15 VDOP 0-99.9
|
||||
f16 TDOP 0-99.9
|
||||
s17 Reserved no data
|
||||
*cc Checksum
|
||||
*/
|
||||
bufptr = (char *)(_rx_buffer + 10);
|
||||
|
||||
/*
|
||||
* Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
|
||||
*/
|
||||
int coordinatesFound = 0;
|
||||
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
|
||||
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
|
||||
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
|
||||
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
|
||||
char ns = '?', ew = '?';
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') {
|
||||
/*
|
||||
* if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
|
||||
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
|
||||
*/
|
||||
lat = strtod(bufptr, &endp);
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') {
|
||||
lon = strtod(bufptr, &endp);
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') {
|
||||
alt = strtod(bufptr, &endp);
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (ns == 'S') {
|
||||
lat = -lat;
|
||||
}
|
||||
|
||||
if (ew == 'W') {
|
||||
lon = -lon;
|
||||
}
|
||||
|
||||
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||
_gps_position->alt = alt * 1000;
|
||||
_rate_count_lat_lon++;
|
||||
|
||||
if (coordinatesFound < 3) {
|
||||
_gps_position->fix_type = 0;
|
||||
|
||||
} else {
|
||||
_gps_position->fix_type = 3 + fix_quality;
|
||||
}
|
||||
|
||||
_gps_position->timestamp_position = hrt_absolute_time();
|
||||
|
||||
double track_rad = track_true * M_PI / 180.0;
|
||||
|
||||
double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */
|
||||
double velocity_north = velocity_ms * cos(track_rad);
|
||||
double velocity_east = velocity_ms * sin(track_rad);
|
||||
|
||||
_gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */
|
||||
_gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */
|
||||
_gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */
|
||||
_gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */
|
||||
_gps_position->cog_rad =
|
||||
track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
||||
_gps_position->c_variance_rad = 0.1;
|
||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
return 1;
|
||||
|
||||
} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
|
||||
/*
|
||||
Position error statistics
|
||||
An example of the GST message string is:
|
||||
|
||||
$GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
|
||||
|
||||
The Talker ID ($--) will vary depending on the satellite system used for the position solution:
|
||||
|
||||
$GP - GPS only
|
||||
$GL - GLONASS only
|
||||
$GN - Combined
|
||||
GST message fields
|
||||
Field Meaning
|
||||
0 Message ID $GPGST
|
||||
1 UTC of position fix
|
||||
2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
|
||||
3 Error ellipse semi-major axis 1 sigma error, in meters
|
||||
4 Error ellipse semi-minor axis 1 sigma error, in meters
|
||||
5 Error ellipse orientation, degrees from true north
|
||||
6 Latitude 1 sigma error, in meters
|
||||
7 Longitude 1 sigma error, in meters
|
||||
8 Height 1 sigma error, in meters
|
||||
9 The checksum data, always begins with *
|
||||
*/
|
||||
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
|
||||
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
|
||||
_gps_position->epv = alt_err;
|
||||
|
||||
_gps_position->s_variance_m_s = 0;
|
||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||
|
||||
} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
|
||||
/*
|
||||
The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
|
||||
|
||||
$GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
|
||||
|
||||
GSV message fields
|
||||
Field Meaning
|
||||
0 Message ID $GPGSV
|
||||
1 Total number of messages of this type in this cycle
|
||||
2 Message number
|
||||
3 Total number of SVs visible
|
||||
4 SV PRN number
|
||||
5 Elevation, in degrees, 90 maximum
|
||||
6 Azimuth, degrees from True North, 000 through 359
|
||||
7 SNR, 00 through 99 dB (null when not tracking)
|
||||
8-11 Information about second SV, same format as fields 4 through 7
|
||||
12-15 Information about third SV, same format as fields 4 through 7
|
||||
16-19 Information about fourth SV, same format as fields 4 through 7
|
||||
20 The checksum data, always begins with *
|
||||
*/
|
||||
/*
|
||||
* currently process only gps, because do not know what
|
||||
* Global satellite ID I should use for non GPS sats
|
||||
*/
|
||||
bool bGPS = false;
|
||||
|
||||
if (memcmp(_rx_buffer, "$GP", 3) != 0) {
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
bGPS = true;
|
||||
}
|
||||
|
||||
int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
|
||||
struct gsv_sat {
|
||||
int svid;
|
||||
int elevation;
|
||||
int azimuth;
|
||||
int snr;
|
||||
} sat[4];
|
||||
memset(sat, 0, sizeof(sat));
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if ((this_msg_num == 0) && (bGPS == true)) {
|
||||
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
|
||||
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
|
||||
memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
|
||||
memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
|
||||
memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
|
||||
}
|
||||
|
||||
int end = 4;
|
||||
|
||||
if (this_msg_num == all_msg_num) {
|
||||
end = tot_sv_visible - (this_msg_num - 1) * 4;
|
||||
_gps_position->satellites_used = tot_sv_visible;
|
||||
_satellite_info->count = SAT_INFO_MAX_SATELLITES;
|
||||
_satellite_info->timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
for (int y = 0 ; y < end ; y++) {
|
||||
if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
|
||||
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
|
||||
_satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false);
|
||||
_satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr;
|
||||
_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
|
||||
_satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int ASHTECH::receive(unsigned timeout)
|
||||
{
|
||||
{
|
||||
/* poll descriptor */
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
uint8_t buf[32];
|
||||
|
||||
/* timeout additional to poll */
|
||||
uint64_t time_started = hrt_absolute_time();
|
||||
|
||||
int j = 0;
|
||||
ssize_t bytes_count = 0;
|
||||
|
||||
while (true) {
|
||||
|
||||
/* pass received bytes to the packet decoder */
|
||||
while (j < bytes_count) {
|
||||
int l = 0;
|
||||
|
||||
if ((l = parse_char(buf[j])) > 0) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message(l) > 0) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* in case we keep trying but only get crap from GPS */
|
||||
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
j++;
|
||||
}
|
||||
|
||||
/* everything is read */
|
||||
j = bytes_count = 0;
|
||||
|
||||
/* then poll for new data */
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2);
|
||||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
return -1;
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* Timeout */
|
||||
return -1;
|
||||
|
||||
} else if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/*
|
||||
* We are here because poll says there is some data, so this
|
||||
* won't block even on a blocking device. If more bytes are
|
||||
* available, we'll go back to poll() again...
|
||||
*/
|
||||
bytes_count = ::read(_fd, buf, sizeof(buf));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
|
||||
|
||||
int ASHTECH::parse_char(uint8_t b)
|
||||
{
|
||||
int iRet = 0;
|
||||
|
||||
switch (_decode_state) {
|
||||
/* First, look for sync1 */
|
||||
case NME_DECODE_UNINIT:
|
||||
if (b == '$') {
|
||||
_decode_state = NME_DECODE_GOT_SYNC1;
|
||||
_rx_buffer_bytes = 0;
|
||||
_rx_buffer[_rx_buffer_bytes++] = b;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case NME_DECODE_GOT_SYNC1:
|
||||
if (b == '$') {
|
||||
_decode_state = NME_DECODE_GOT_SYNC1;
|
||||
_rx_buffer_bytes = 0;
|
||||
|
||||
} else if (b == '*') {
|
||||
_decode_state = NME_DECODE_GOT_ASTERIKS;
|
||||
}
|
||||
|
||||
if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
|
||||
_decode_state = NME_DECODE_UNINIT;
|
||||
_rx_buffer_bytes = 0;
|
||||
|
||||
} else {
|
||||
_rx_buffer[_rx_buffer_bytes++] = b;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case NME_DECODE_GOT_ASTERIKS:
|
||||
_rx_buffer[_rx_buffer_bytes++] = b;
|
||||
_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
|
||||
break;
|
||||
|
||||
case NME_DECODE_GOT_FIRST_CS_BYTE:
|
||||
_rx_buffer[_rx_buffer_bytes++] = b;
|
||||
uint8_t checksum = 0;
|
||||
uint8_t *buffer = _rx_buffer + 1;
|
||||
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
|
||||
|
||||
for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
|
||||
|
||||
if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
|
||||
(HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
|
||||
iRet = _rx_buffer_bytes;
|
||||
}
|
||||
|
||||
_decode_state = NME_DECODE_UNINIT;
|
||||
_rx_buffer_bytes = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
return iRet;
|
||||
}
|
||||
|
||||
void ASHTECH::decode_init(void)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* ashtech board configuration script
|
||||
*/
|
||||
|
||||
const char comm[] = "$PASHS,POP,20\r\n"\
|
||||
"$PASHS,NME,ZDA,B,ON,3\r\n"\
|
||||
"$PASHS,NME,GGA,B,OFF\r\n"\
|
||||
"$PASHS,NME,GST,B,ON,3\r\n"\
|
||||
"$PASHS,NME,POS,B,ON,0.05\r\n"\
|
||||
"$PASHS,NME,GSV,B,ON,3\r\n"\
|
||||
"$PASHS,SPD,A,8\r\n"\
|
||||
"$PASHS,SPD,B,9\r\n";
|
||||
|
||||
int ASHTECH::configure(unsigned &baudrate)
|
||||
{
|
||||
/* try different baudrates */
|
||||
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
|
||||
|
||||
|
||||
for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
|
||||
baudrate = baudrates_to_try[baud_i];
|
||||
set_baudrate(_fd, baudrate);
|
||||
write(_fd, (uint8_t *)comm, sizeof(comm));
|
||||
}
|
||||
|
||||
set_baudrate(_fd, 115200);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013. All rights reserved.
|
||||
* Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
|
||||
* Kistanov Alexander <akistanov@gramant.ru>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file ASHTECH protocol definitions */
|
||||
|
||||
#ifndef ASHTECH_H_
|
||||
#define ASHTECH_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#ifndef RECV_BUFFER_SIZE
|
||||
#define RECV_BUFFER_SIZE 512
|
||||
|
||||
#define SAT_INFO_MAX_SATELLITES 20
|
||||
#endif
|
||||
|
||||
|
||||
class ASHTECH : public GPS_Helper
|
||||
{
|
||||
enum ashtech_decode_state_t {
|
||||
NME_DECODE_UNINIT,
|
||||
NME_DECODE_GOT_SYNC1,
|
||||
NME_DECODE_GOT_ASTERIKS,
|
||||
NME_DECODE_GOT_FIRST_CS_BYTE
|
||||
};
|
||||
|
||||
int _fd;
|
||||
struct satellite_info_s *_satellite_info;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
int ashtechlog_fd;
|
||||
|
||||
ashtech_decode_state_t _decode_state;
|
||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||
uint16_t _rx_buffer_bytes;
|
||||
bool _parse_error; /** parse error flag */
|
||||
char *_parse_pos; /** parse position */
|
||||
|
||||
bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */
|
||||
/* int _satellites_count; **< Number of satellites info parsed. */
|
||||
uint8_t count; /**< Number of satellites in satellite info */
|
||||
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
|
||||
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
|
||||
uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
|
||||
|
||||
public:
|
||||
ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
|
||||
~ASHTECH();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
void decode_init(void);
|
||||
int handle_message(int len);
|
||||
int parse_char(uint8_t b);
|
||||
/** Read int ASHTECH parameter */
|
||||
int32_t read_int();
|
||||
/** Read float ASHTECH parameter */
|
||||
double read_float();
|
||||
/** Read char ASHTECH parameter */
|
||||
char read_char();
|
||||
|
||||
};
|
||||
|
||||
#endif /* ASHTECH_H_ */
|
||||
|
|
@ -69,6 +69,7 @@
|
|||
|
||||
#include "ubx.h"
|
||||
#include "mtk.h"
|
||||
#include "ashtech.h"
|
||||
|
||||
|
||||
#define TIMEOUT_5HZ 500
|
||||
|
|
@ -341,6 +342,10 @@ GPS::task_main()
|
|||
_Helper = new MTK(_serial_fd, &_report_gps_pos);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -402,6 +407,10 @@ GPS::task_main()
|
|||
mode_str = "MTK";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
mode_str = "ASHTECH";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -429,6 +438,10 @@ GPS::task_main()
|
|||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_ASHTECH;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
|
|
@ -475,6 +488,10 @@ GPS::print_info()
|
|||
warnx("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
warnx("protocol: ASHTECH");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ MODULE_COMMAND = gps
|
|||
SRCS = gps.cpp \
|
||||
gps_helper.cpp \
|
||||
mtk.cpp \
|
||||
ashtech.cpp \
|
||||
ubx.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
|
|
|||
|
|
@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
|
|||
|
||||
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
|
||||
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
|
||||
esc.esc[0].esc_temperature = msg.temperature1 - 20;
|
||||
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
|
||||
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
|
||||
esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
|
||||
esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
|
||||
esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
|
||||
|
||||
/* announce the esc if needed, just publish else */
|
||||
if (_esc_pub > 0) {
|
||||
|
|
@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
|
|||
msg.gam_sensor_id = GAM_SENSOR_ID;
|
||||
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
|
||||
|
||||
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
|
||||
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
|
||||
msg.temperature2 = 20; // 0 deg. C.
|
||||
|
||||
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
|
||||
const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
|
||||
msg.main_voltage_L = (uint8_t)voltage & 0xff;
|
||||
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
|
||||
|
||||
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
|
||||
const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
|
||||
msg.current_L = (uint8_t)current & 0xff;
|
||||
msg.current_H = (uint8_t)(current >> 8) & 0xff;
|
||||
|
||||
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
|
||||
const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
|
||||
msg.rpm_L = (uint8_t)rpm & 0xff;
|
||||
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
|
||||
|
||||
|
|
|
|||
|
|
@ -600,8 +600,8 @@ MK::task_main()
|
|||
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
|
||||
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
|
||||
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
|
||||
esc.esc[i].esc_voltage = (uint16_t) 0;
|
||||
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
|
||||
esc.esc[i].esc_voltage = 0.0F;
|
||||
esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
|
||||
esc.esc[i].esc_rpm = (uint16_t) 0;
|
||||
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
|
||||
|
||||
|
|
@ -614,7 +614,7 @@ MK::task_main()
|
|||
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
|
||||
}
|
||||
|
||||
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
|
||||
esc.esc[i].esc_temperature = static_cast<float>(Motor[i].Temperature);
|
||||
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
|
||||
esc.esc[i].esc_errorcount = (uint16_t) 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -546,7 +546,7 @@ SF0X::collect()
|
|||
float si_units;
|
||||
bool valid = false;
|
||||
|
||||
for (unsigned i = 0; i < ret; i++) {
|
||||
for (int i = 0; i < ret; i++) {
|
||||
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
|
||||
valid = true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
|
|||
crosstrack_error->distance = 0.0f;
|
||||
crosstrack_error->bearing = 0.0f;
|
||||
|
||||
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
// Return error if arguments are bad
|
||||
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
|
||||
if (dist_to_end < 0.1f) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
|
||||
|
|
@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
|
|||
return return_value;
|
||||
}
|
||||
|
||||
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
|
||||
|
||||
if (sin(bearing_diff) >= 0) {
|
||||
|
|
@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
|||
crosstrack_error->bearing = 0.0f;
|
||||
|
||||
// Return error if arguments are bad
|
||||
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
|
||||
if (radius < 0.1f) { return return_value; }
|
||||
|
||||
|
||||
if (arc_sweep >= 0) {
|
||||
if (arc_sweep >= 0.0f) {
|
||||
bearing_sector_start = arc_start_bearing;
|
||||
bearing_sector_end = arc_start_bearing + arc_sweep;
|
||||
|
||||
|
|
@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
|||
|
||||
double start_disp_x = (double)radius * sin(arc_start_bearing);
|
||||
double start_disp_y = (double)radius * cos(arc_start_bearing);
|
||||
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double lon_start = lon_now + start_disp_x / 111111.0;
|
||||
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
|
||||
double lon_end = lon_now + end_disp_x / 111111.0;
|
||||
|
|
@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
|||
|
||||
}
|
||||
|
||||
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
|
||||
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
|
||||
return_value = OK;
|
||||
return return_value;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -241,15 +241,15 @@ struct log_GPSP_s {
|
|||
#define LOG_ESC_MSG 18
|
||||
struct log_ESC_s {
|
||||
uint16_t counter;
|
||||
uint8_t esc_count;
|
||||
uint8_t esc_connectiontype;
|
||||
uint8_t esc_num;
|
||||
uint8_t esc_count;
|
||||
uint8_t esc_connectiontype;
|
||||
uint8_t esc_num;
|
||||
uint16_t esc_address;
|
||||
uint16_t esc_version;
|
||||
uint16_t esc_voltage;
|
||||
uint16_t esc_current;
|
||||
uint16_t esc_rpm;
|
||||
uint16_t esc_temperature;
|
||||
float esc_voltage;
|
||||
float esc_current;
|
||||
int32_t esc_rpm;
|
||||
float esc_temperature;
|
||||
float esc_setpoint;
|
||||
uint16_t esc_setpoint_raw;
|
||||
};
|
||||
|
|
@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
|
||||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
|
|
|
|||
|
|
@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE {
|
|||
|
||||
/**
|
||||
* Electronic speed controller status.
|
||||
* Unsupported float fields should be assigned NaN.
|
||||
*/
|
||||
struct esc_status_s {
|
||||
/* use of a counter and timestamp recommended (but not necessary) */
|
||||
|
|
@ -89,17 +90,17 @@ struct esc_status_s {
|
|||
enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */
|
||||
|
||||
struct {
|
||||
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
|
||||
enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */
|
||||
uint16_t esc_version; /**< Version of current ESC - if supported */
|
||||
uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */
|
||||
uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */
|
||||
uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */
|
||||
uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */
|
||||
float esc_setpoint; /**< setpoint of current ESC */
|
||||
uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
|
||||
int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */
|
||||
float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */
|
||||
float esc_current; /**< Current measured from current ESC [A] - if supported */
|
||||
float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */
|
||||
float esc_setpoint; /**< setpoint of current ESC */
|
||||
uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */
|
||||
uint16_t esc_state; /**< State of ESC - depend on Vendor */
|
||||
uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
|
||||
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
|
||||
uint16_t esc_version; /**< Version of current ESC - if supported */
|
||||
uint16_t esc_state; /**< State of ESC - depend on Vendor */
|
||||
} esc[CONNECTED_ESC_MAX];
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
|
|||
uint64_t timestamp_variance;
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float c_variance_rad; /**< course accuracy estimate rad */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
|
||||
float eph; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv; /**< GPS VDOP horizontal dilution of position in m */
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ int UavcanEscController::init()
|
|||
|
||||
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||
{
|
||||
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
|
||||
if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
|
||||
perf_count(_perfcnt_invalid_input);
|
||||
return;
|
||||
}
|
||||
|
|
@ -93,25 +93,24 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
|||
*/
|
||||
uavcan::equipment::esc::RawCommand msg;
|
||||
|
||||
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
|
||||
|
||||
if (_armed) {
|
||||
for (unsigned i = 0; i < num_outputs; i++) {
|
||||
|
||||
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
|
||||
if (scaled < 1.0F) {
|
||||
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
|
||||
}
|
||||
|
||||
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
|
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
|
||||
if (scaled > cmd_max) {
|
||||
scaled = cmd_max;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
|
||||
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
} else {
|
||||
; // Correct value
|
||||
}
|
||||
|
||||
msg.cmd.push_back(static_cast<unsigned>(scaled));
|
||||
msg.cmd.push_back(static_cast<int>(scaled));
|
||||
|
||||
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -129,10 +128,31 @@ void UavcanEscController::arm_esc(bool arm)
|
|||
|
||||
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||
{
|
||||
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
|
||||
if (msg.esc_index < CONNECTED_ESC_MAX) {
|
||||
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
|
||||
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
|
||||
auto &ref = _esc_status.esc[msg.esc_index];
|
||||
|
||||
ref.esc_address = msg.getSrcNodeID().get();
|
||||
|
||||
ref.esc_voltage = msg.voltage;
|
||||
ref.esc_current = msg.current;
|
||||
ref.esc_temperature = msg.temperature;
|
||||
ref.esc_setpoint = msg.power_rating_pct;
|
||||
ref.esc_rpm = msg.rpm;
|
||||
ref.esc_errorcount = msg.error_count;
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
|
||||
{
|
||||
// TODO publish to ORB
|
||||
_esc_status.counter += 1;
|
||||
_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
|
||||
|
||||
if (_esc_status_pub > 0) {
|
||||
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
|
||||
} else {
|
||||
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,6 +48,8 @@
|
|||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/esc/Status.hpp>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
|
|
@ -73,9 +75,8 @@ private:
|
|||
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
|
||||
|
||||
|
||||
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
|
||||
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
|
||||
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController*,
|
||||
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
|
||||
|
|
@ -84,6 +85,10 @@ private:
|
|||
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
|
||||
TimerCbBinder;
|
||||
|
||||
bool _armed = false;
|
||||
esc_status_s _esc_status = {};
|
||||
orb_advert_t _esc_status_pub = -1;
|
||||
|
||||
/*
|
||||
* libuavcan related things
|
||||
*/
|
||||
|
|
@ -93,12 +98,6 @@ private:
|
|||
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
|
||||
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
|
||||
|
||||
/*
|
||||
* ESC states
|
||||
*/
|
||||
bool _armed = false;
|
||||
uavcan::equipment::esc::Status _states[MAX_ESCS];
|
||||
|
||||
/*
|
||||
* Perf counters
|
||||
*/
|
||||
|
|
|
|||
2
uavcan
2
uavcan
|
|
@ -1 +1 @@
|
|||
Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01
|
||||
Subproject commit 4de0338824972de4d3a8e29697ea1a2d95a926c0
|
||||
Loading…
Reference in New Issue