zhangzhuo/M300CO2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

1.添加了EGM96-5的数据集和算法支持，可以正确求解GEOID Offset高度。

Browse Source 
2.在同步时间时现在会将起飞点的海拔高度写回到配置文件/home/data/Settings/MainSettings.ini键值为WBACK/HeightOfHomePoint
3.现在的高度回调函数以及获取函数调整为了DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED。
This commit is contained in:
zhangzhuo
2023-04-07 15:46:09 +08:00
parent a714945d2d
commit 09a5436ad7
9 changed files with 65752 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

259
Source/EGM96/EGM96.c Normal file
View File

@ -0,0 +1,259 @@
/*
* Copyright (c) 2006 D.Ineiev <ineiev@yahoo.co.uk>
* Copyright (c) 2020 Emeric Grange <emeric.grange@gmail.com>
*
* This software is provided 'as-is', without any express or implied warranty.
* In no event will the authors be held liable for any damages arising from
* the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
/*
* This program is designed for the calculation of a geoid undulation at a point
* whose latitude and longitude is specified.
*
* This program is designed to be used with the constants of EGM96 and those of
* the WGS84(g873) system. The undulation will refer to the WGS84 ellipsoid.
*
* It's designed to use the potential coefficient model EGM96 and a set of
* spherical harmonic coefficients of a correction term.
* The correction term is composed of several different components, the primary
* one being the conversion of a height anomaly to a geoid undulation.
* The principles of this procedure were initially described in the paper:
* - use of potential coefficient models for geoid undulation determination using
* a spherical harmonic representation of the height anomaly/geoid undulation
* difference by R.H. Rapp, Journal of Geodesy, 1996.
*
* This program is a modification of the program described in the following report:
* - a fortran program for the computation of gravimetric quantities from high
* degree spherical harmonic expansions, Richard H. Rapp, report 334, Department
* of Geodetic Science and Surveying, the Ohio State University, Columbus, 1982
*/
#include "EGM96.h"
#include "EGM96_data.h"
#include <math.h>
/* ************************************************************************** */
#define _coeffs (65341) //!< Size of correction and harmonic coefficients arrays (361*181)
#define _nmax (360) //!< Maximum degree and orders of harmonic coefficients.
#define _361 (361)
/* ************************************************************************** */
double hundu(double p[_coeffs+1],
double sinml[_361+1], double cosml[_361+1],
double gr, double re)
{
// WGS 84 gravitational constant in m³/s² (mass of Earth’s atmosphere included)
const double GM = 0.3986004418e15;
// WGS 84 datum surface equatorial radius
const double ae = 6378137.0;
double ar = ae/re;
double arn = ar;
double ac = 0;
double a = 0;
unsigned k = 3;
for (unsigned n = 2; n <= _nmax; n++)
{
arn *= ar;
k++;
double sum = p[k]*egm96_data[k][2];
double sumc = p[k]*egm96_data[k][0];
for (unsigned m = 1; m <= n; m++)
{
k++;
double tempc = egm96_data[k][0]*cosml[m] + egm96_data[k][1]*sinml[m];
double temp = egm96_data[k][2]*cosml[m] + egm96_data[k][3]*sinml[m];
sumc += p[k]*tempc;
sum += p[k]*temp;
}
ac += sumc;
a += sum*arn;
}
ac += egm96_data[1][0] + (p[2]*egm96_data[2][0]) + (p[3] * (egm96_data[3][0]*cosml[1] + egm96_data[3][1]*sinml[1]));
// Add haco = ac/100 to convert height anomaly on the ellipsoid to the undulation
// Add -0.53m to make undulation refer to the WGS84 ellipsoid
return ((a * GM) / (gr * re)) + (ac / 100.0) - 0.53;
}
void dscml(double rlon, double sinml[_361+1], double cosml[_361+1])
{
double a = sin(rlon);
double b = cos(rlon);
sinml[1] = a;
cosml[1] = b;
sinml[2] = 2*b*a;
cosml[2] = 2*b*b - 1;
for (unsigned m = 3; m <= _nmax; m++)
{
sinml[m] = 2*b*sinml[m-1] - sinml[m-2];
cosml[m] = 2*b*cosml[m-1] - cosml[m-2];
}
}
/*!
* \param m: order.
* \param theta: Colatitude (radians).
* \param rleg: Normalized legendre function.
*
* This subroutine computes all normalized legendre function in 'rleg'.
* The dimensions of array 'rleg' must be at least equal to nmax+1.
* All calculations are in double precision.
*
* Original programmer: Oscar L. Colombo, Dept. of Geodetic Science the Ohio State University, August 1980.
* ineiev: I removed the derivatives, for they are never computed here.
*/
void legfdn(unsigned m, double theta, double rleg[_361+1])
{
static double drts[1301], dirt[1301], cothet, sithet, rlnn[_361+1];
static int ir; // TODO 'ir' must be set to zero before the first call to this sub.
unsigned nmax1 = _nmax + 1;
unsigned nmax2p = (2 * _nmax) + 1;
unsigned m1 = m + 1;
unsigned m2 = m + 2;
unsigned m3 = m + 3;
unsigned n, n1, n2;
if (!ir)
{
ir = 1;
for (n = 1; n <= nmax2p; n++)
{
drts[n] = sqrt(n);
dirt[n] = 1 / drts[n];
}
}
cothet = cos(theta);
sithet = sin(theta);
// compute the legendre functions
rlnn[1] = 1;
rlnn[2] = sithet * drts[3];
for (n1 = 3; n1 <= m1; n1++)
{
n = n1 - 1;
n2 = 2 * n;
rlnn[n1] = drts[n2 + 1] * dirt[n2] * sithet * rlnn[n];
}
switch (m)
{
case 1:
rleg[2] = rlnn[2];
rleg[3] = drts[5] * cothet * rleg[2];
break;
case 0:
rleg[1] = 1;
rleg[2] = cothet * drts[3];
break;
}
rleg[m1] = rlnn[m1];
if (m2 <= nmax1)
{
rleg[m2] = drts[m1*2 + 1] * cothet * rleg[m1];
if (m3 <= nmax1)
{
for (n1 = m3; n1 <= nmax1; n1++)
{
n = n1 - 1;
if ((!m && n < 2) || (m == 1 && n < 3)) continue;
n2 = 2 * n;
rleg[n1] = drts[n2+1] * dirt[n+m] * dirt[n-m] * (drts[n2-1] * cothet * rleg[n1-1] - drts[n+m-1] * drts[n-m-1] * dirt[n2-3] * rleg[n1-2]);
}
}
}
}
/*!
* \param lat: Latitude in radians.
* \param lon: Longitude in radians.
* \param re: Geocentric radius.
* \param rlat: Geocentric latitude.
* \param gr: Normal gravity (m/sec²).
*
* This subroutine computes geocentric distance to the point, the geocentric
* latitude, and an approximate value of normal gravity at the point based the
* constants of the WGS84(g873) system are used.
*/
void radgra(double lat, double lon, double *rlat, double *gr, double *re)
{
const double a = 6378137.0;
const double e2 = 0.00669437999013;
const double geqt = 9.7803253359;
const double k = 0.00193185265246;
double t1 = sin(lat) * sin(lat);
double n = a / sqrt(1.0 - (e2 * t1));
double t2 = n * cos(lat);
double x = t2 * cos(lon);
double y = t2 * sin(lon);
double z = (n * (1 - e2)) * sin(lat);
*re = sqrt((x * x) + (y * y) + (z * z)); // compute the geocentric radius
*rlat = atan(z / sqrt((x * x) + (y * y))); // compute the geocentric latitude
*gr = geqt * (1 + (k * t1)) / sqrt(1 - (e2 * t1)); // compute normal gravity (m/sec²)
}
/*!
* \brief Compute the geoid undulation from the EGM96 potential coefficient model, for a given latitude and longitude.
* \param lat: Latitude in radians.
* \param lon: Longitude in radians.
* \return The geoid undulation / altitude offset (in meters).
*/
double undulation(double lat, double lon)
{
double p[_coeffs+1], sinml[_361+1], cosml[_361+1], rleg[_361+1];
double rlat, gr, re;
unsigned nmax1 = _nmax + 1;
// compute the geocentric latitude, geocentric radius, normal gravity
radgra(lat, lon, &rlat, &gr, &re);
rlat = (M_PI / 2) - rlat;
for (unsigned j = 1; j <= nmax1; j++)
{
unsigned m = j - 1;
legfdn(m, rlat, rleg);
for (unsigned i = j ; i <= nmax1; i++)
{
p[(((i - 1) * i) / 2) + m + 1] = rleg[i];
}
}
dscml(lon, sinml, cosml);
return hundu(p, sinml, cosml, gr, re);
}
/* ************************************************************************** */
double egm96_compute_altitude_offset(double lat, double lon)
{
const double rad = (180.0 / M_PI);
return undulation(lat/rad, lon/rad);
}
/* ************************************************************************** */

39
Source/EGM96/EGM96.h Normal file
View File

@ -0,0 +1,39 @@
/*
* Copyright (c) 2006 D.Ineiev <ineiev@yahoo.co.uk>
* Copyright (c) 2020 Emeric Grange <emeric.grange@gmail.com>
*
* This software is provided 'as-is', without any express or implied warranty.
* In no event will the authors be held liable for any damages arising from
* the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#ifndef EGM96_H
#define EGM96_H
/* ************************************************************************** */
#ifdef __cplusplus
extern "C" {
#endif
/*!
* \brief Compute the geoid undulation from the EGM96 potential coefficient model, for a given latitude and longitude.
* \param latitude: Latitude (in degrees).
* \param longitude: Longitude (in degrees).
* \return The geoid undulation / altitude offset (in meters).
*/
double egm96_compute_altitude_offset(double latitude, double longitude);
#ifdef __cplusplus
}
#endif
/* ************************************************************************** */
#endif // EGM96_H

65372
Source/EGM96/EGM96_data.h Normal file
View File

File diff suppressed because it is too large Load Diff

49
Source/M300/PSDK_Qt/Main/VehicleController.cpp
View File

@ -31,6 +31,7 @@ VehicleController::VehicleController(QObject* parent /*= nullptr*/)
m_iFlagIsVehicleCapturing = 0;
m_iFlagIsCaptureModeInited = 0;
m_fHeightOfHomePoint = -1;
//m_cCaptureMode = 0;
}
@ -46,11 +47,11 @@ T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveFlightStatusCall
T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveHeightCallback(const uint8_t* data, uint16_t dataSize, const T_DjiDataTimestamp* timestamp)
{
float fHeight = *((T_DjiFcSubscriptionHeightFusion*)data);
float fHeight = *((T_DjiFcSubscriptionAltitudeFused*)data);
//qDebug() <<"[Fused Height]:"<< fHeight;
///Pump need to be added
if (m_siFlagIsPumpWorking==0&& fHeight>10)
if (m_siFlagIsPumpWorking==0&& fHeight > 10)
{
//system("sudo gpio mode 7 out");
m_siFlagIsPumpWorking = 1;
@ -319,7 +320,7 @@ int VehicleController::StartupPSDK_M300RTK()
//osalHandler->TaskSleepMs(5000);
SetupMessagePipe();
SetupWaypointStatusCallback();
InitSystemDateTime();
InitSystemParams();
SetupSubscriptions();
@ -398,11 +399,11 @@ int VehicleController::SetupSubscriptions()
// qDebug() << "init data subscription module finished.";
// }
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION error.";
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED error.";
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
@ -531,9 +532,11 @@ int VehicleController::SetupWidget()
return 0;
}
int VehicleController::InitSystemDateTime()
int VehicleController::InitSystemParams()
{
T_DjiReturnCode tDjiReturnCode;
QString qstrWBackPath("/home/data/Settings/MainSettings.ini");
///Init
tDjiReturnCode = DjiFcSubscription_Init();
@ -547,7 +550,7 @@ int VehicleController::InitSystemDateTime()
qDebug() << "InitSystemDateTime finished.";
}
///SubscribeTopic
///SubscribeTopic GPS_SIGNAL_LEVEL
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
@ -556,6 +559,7 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///SubscribeTopic GPS_DATE
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
@ -564,6 +568,7 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///SubscribeTopic GPS_TIME
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
@ -572,6 +577,15 @@ int VehicleController::InitSystemDateTime()
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///SubscribeTopic ALTITUDE_OF_HOMEPOINT
tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT error.";
return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
}
///Make sure Gps avalible
T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
bool bStop = false;
@ -617,6 +631,8 @@ int VehicleController::InitSystemDateTime()
///Get GPS Datetime
T_DjiFcSubscriptionGpsDate tGpsDate = { 0 };
T_DjiFcSubscriptionGpsTime tGpsTime = { 0 };
T_DjiFcSubscriptionAltitudeOfHomePoint tAltOfHP = { 0 };
tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, (uint8_t*)&tGpsDate, sizeof(T_DjiFcSubscriptionGpsDate), &tTimestamp);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
@ -632,6 +648,18 @@ int VehicleController::InitSystemDateTime()
return -2;
}
tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, (uint8_t*)&tAltOfHP, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint), &tTimestamp);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
return -3;
}
m_fHeightOfHomePoint = tAltOfHP;
//////////////////////////////////////////////////////////////////////////write back AOHP
QSettings qsMainSettings(qstrWBackPath, QSettings::Format::IniFormat);
qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint"), m_fHeightOfHomePoint);
//////////////////////////////////////////////////////////////////////////
//qDebug() << "Date" << tGpsDate << "Time" << tGpsTime;
QDateTime qdtDateTime;
@ -695,6 +723,13 @@ int VehicleController::InitSystemDateTime()
return -13;
}
tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT);
if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
return -14;
}
return 0;
}

5
Source/M300/PSDK_Qt/Main/VehicleController.h
View File

@ -41,7 +41,8 @@ private:
int m_iFlagIsVehicleTakeoff;
int m_iFlagIsVehicleCapturing;
int m_iFlagIsCaptureModeInited;
float m_fHeightOfHomePoint;
public:
/// test func Do Not Call
static T_DjiReturnCode DjiTest_FcSubscriptionReceiveFlightStatusCallback(const uint8_t* ccData, uint16_t sDataSize,const T_DjiDataTimestamp* tDjiTimestamp);
@ -68,7 +69,7 @@ private:
int SetupWidget();
///
public:///for test
int InitSystemDateTime();
int InitSystemParams();
int SetupSubscriptions();
///
public:///for test

2
Source/MainAcqThread.cpp
View File

@ -197,7 +197,7 @@ int CMainAcqThread::GetData()
m_struUASDataFrame.fWindDirection = fAngle;
m_struUASDataFrame.fWindSpeed = fSpeed;
m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);
m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
int CMainAcqThread::OnTimerCapture()