diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-16 15:53:01 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-16 15:53:01 +0200 |
commit | 17a1c986c2fa7c49197d14c1b1ef0fc494d9e22f (patch) | |
tree | 11ffaa6545fd35a63bce1bc98369f993c8de9e74 | |
parent | 4bfa30bfd6151c63c4cd8e2ece51e5568f4f3d16 (diff) | |
parent | 3eb68bc66000e01849b562e2e3f7a077e1668203 (diff) | |
download | px4-firmware-17a1c986c2fa7c49197d14c1b1ef0fc494d9e22f.tar.gz px4-firmware-17a1c986c2fa7c49197d14c1b1ef0fc494d9e22f.tar.bz2 px4-firmware-17a1c986c2fa7c49197d14c1b1ef0fc494d9e22f.zip |
Merge branch 'master' of github.com:PX4/Firmware
-rw-r--r-- | Tools/px_romfs_pruner.py | 2 | ||||
-rw-r--r-- | src/drivers/drv_gps.h | 3 | ||||
-rw-r--r-- | src/drivers/drv_pwm_output.h | 2 | ||||
-rw-r--r-- | src/drivers/gps/ashtech.cpp | 641 | ||||
-rw-r--r-- | src/drivers/gps/ashtech.h | 96 | ||||
-rw-r--r-- | src/drivers/gps/gps.cpp | 17 | ||||
-rw-r--r-- | src/drivers/gps/module.mk | 1 | ||||
-rw-r--r-- | src/drivers/hott/messages.cpp | 14 | ||||
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 6 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 2 | ||||
-rw-r--r-- | src/lib/geo/geo.c | 17 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 16 | ||||
-rw-r--r-- | src/modules/uORB/topics/esc_status.h | 19 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_gps_position.h | 2 | ||||
-rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 44 | ||||
-rw-r--r-- | src/modules/uavcan/actuators/esc.hpp | 17 | ||||
m--------- | uavcan | 0 |
17 files changed, 839 insertions, 60 deletions
diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index fcc40b09e..aef1cc7a3 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -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) diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index e14f4e00d..76a211000 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -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; diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bc586f395..f66ec7c95 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -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) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp new file mode 100644 index 000000000..a2e292de2 --- /dev/null +++ b/src/drivers/gps/ashtech.cpp @@ -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; +} diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h new file mode 100644 index 000000000..6ba522b9c --- /dev/null +++ b/src/drivers/gps/ashtech.h @@ -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_ */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 34dd63086..5fb4b9ff8 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -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; } diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index b00818424..4f99b0d3b 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = gps SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ + ashtech.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 086132573..f1b12b067 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -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; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3996b76a6..1055487cb 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -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; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 801bcf40a..fdd524189 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -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; } diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1c8d2a2a7..f595467b3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -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; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d2845eb75..d49fc0c79 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 628824efa..b45c49788 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -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]; }; diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 80d65cd69..31e616f4f 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -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 */ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 223d94731..1990651ef 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -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; - perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled > cmd_max) { + scaled = 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); + } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index cf0988210..f4a3877e6 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -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 */ @@ -94,12 +99,6 @@ private: uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - - /* * Perf counters */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); diff --git a/uavcan b/uavcan -Subproject 01d5bb242a6197e17e0ed466ed86e7ba42bd7d0 +Subproject 4de0338824972de4d3a8e29697ea1a2d95a926c |