dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Ashtech driver: Avoid unnecessary double precision conversion calls

This commit is contained in:
Lorenz Meier 2015-02-08 12:21:33 +01:00
parent e5e42650c4
commit 3bb0008af5
3 changed files with 49 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/drivers/gps/ashtech.cpp
View File

@ -68,10 +68,10 @@ int ASHTECH::handle_message(int len)
7 The checksum data, always begins with * 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. 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; unsigned long long ashtech_time = 0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 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) != ',') { ashtech_time = static_cast<unsigned long long>(strtod(bufptr, &endp)); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
@ -100,7 +100,7 @@ int ASHTECH::handle_message(int len)
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) { if (epoch > GPS_EPOCH_SECS) {
uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1e6; uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1000000;
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
// and control its drift. Since we rely on the HRT for our monotonic // and control its drift. Since we rely on the HRT for our monotonic
@ -191,9 +191,10 @@ int ASHTECH::handle_message(int len)
lon = -lon; lon = -lon;
} }
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; /* convert from degrees, minutes and seconds to degrees * 1e7 */
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; _gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = alt * 1000; _gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast<int>(alt * 1000);
_rate_count_lat_lon++; _rate_count_lat_lon++;
if (fix_quality <= 0) { if (fix_quality <= 0) {
@ -221,7 +222,7 @@ int ASHTECH::handle_message(int len)
_gps_position->cog_rad = _gps_position->cog_rad =
0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ 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->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1; _gps_position->c_variance_rad = 0.1f;
_gps_position->timestamp_velocity = hrt_absolute_time(); _gps_position->timestamp_velocity = hrt_absolute_time();
return 1; return 1;
@ -323,9 +324,9 @@ int ASHTECH::handle_message(int len)
lon = -lon; lon = -lon;
} }
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; _gps_position->lat = static_cast<int>((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->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = alt * 1000; _gps_position->alt = static_cast<int>(alt * 1000);
_rate_count_lat_lon++; _rate_count_lat_lon++;
if (coordinatesFound < 3) { if (coordinatesFound < 3) {
@ -337,20 +338,19 @@ int ASHTECH::handle_message(int len)
_gps_position->timestamp_position = hrt_absolute_time(); _gps_position->timestamp_position = hrt_absolute_time();
double track_rad = track_true * M_PI / 180.0; float track_rad = static_cast<float>(track_true) * M_PI_F / 180.0f;
double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ float velocity_ms = static_cast<float>(ground_speed) / 1.9438445f; /** knots to m/s */
double velocity_north = velocity_ms * cos(track_rad); float velocity_north = static_cast<float>(velocity_ms) * cosf(track_rad);
double velocity_east = velocity_ms * sin(track_rad); float velocity_east = static_cast<float>(velocity_ms) * sinf(track_rad);
_gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */ _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_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_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->vel_d_m_s = static_cast<float>(-vertic_vel); /** GPS ground speed in m/s */
_gps_position->cog_rad = _gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
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->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f;
_gps_position->c_variance_rad = 0.1;
_gps_position->timestamp_velocity = hrt_absolute_time(); _gps_position->timestamp_velocity = hrt_absolute_time();
return 1; return 1;
@ -398,8 +398,9 @@ int ASHTECH::handle_message(int len)
if (bufptr && *(++bufptr) != ',') { alt_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->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
_gps_position->epv = alt_err; + static_cast<float>(lon_err) * static_cast<float>(lon_err));
_gps_position->epv = static_cast<float>(alt_err);
_gps_position->s_variance_m_s = 0; _gps_position->s_variance_m_s = 0;
_gps_position->timestamp_variance = hrt_absolute_time(); _gps_position->timestamp_variance = hrt_absolute_time();

22
src/drivers/gps/mtk.cpp
View File

@ -259,7 +259,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->fix_type = packet.fix_type; _gps_position->fix_type = packet.fix_type;
_gps_position->eph = packet.hdop / 100.0f; // from cm to m _gps_position->eph = packet.hdop / 100.0f; // from cm to m
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_used = packet.satellites; _gps_position->satellites_used = packet.satellites;
@ -267,17 +267,17 @@ MTK::handle_message(gps_mtk_packet_t &packet)
struct tm timeinfo; struct tm timeinfo;
uint32_t timeinfo_conversion_temp; uint32_t timeinfo_conversion_temp;
timeinfo.tm_mday = packet.date * 1e-4; timeinfo.tm_mday = packet.date / 10000;
timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000;
timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1;
timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100;
timeinfo.tm_hour = packet.utc_time * 1e-7; timeinfo.tm_hour = (packet.utc_time / 10000000);
timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000;
timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; timeinfo.tm_min = timeinfo_conversion_temp / 100000;
timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; timeinfo_conversion_temp -= timeinfo.tm_min * 100000;
timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; timeinfo.tm_sec = timeinfo_conversion_temp / 1000;
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; timeinfo_conversion_temp -= timeinfo.tm_sec * 1000;
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) { if (epoch > GPS_EPOCH_SECS) {

21
src/lib/mathlib/math/Quaternion.hpp
View File

@ -1,9 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -38,6 +35,10 @@
* @file Quaternion.hpp * @file Quaternion.hpp
* *
* Quaternion class * Quaternion class
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/ */
#ifndef QUATERNION_HPP #ifndef QUATERNION_HPP
@ -124,10 +125,14 @@ public:
double sinTheta_2 = sin(double(pitch) / 2.0); double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0); double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0); double sinPsi_2 = sin(double(yaw) / 2.0);
data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; /* operations executed in double to avoid loss of precision through
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; * consecutive multiplications. Result stored as float.
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; */
data[0] = static_cast<float>(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2);
data[1] = static_cast<float>(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2);
data[2] = static_cast<float>(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2);
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
} }
void from_dcm(const Matrix<3, 3> &m) { void from_dcm(const Matrix<3, 3> &m) {