BetaFlight深入传感设计之四：GPS传感模块

Posted 2022-10-15 lida2003

BetaFlight深入传感设计之四：GPS传感模块

• 1. HwPreInit/HwInit阶段
• • 1.1 【业务HwPreInit】gpsInit
  • 1.2 【业务HwInit】gpsInitHardware
• 2. HwIo阶段
• 3. HwDataAnalysis阶段
• • 3.1 gpsNewFrame
  • • 3.1.1 【NMEA】gpsNewFrameNMEA
    • 3.1.2 【UBLOX】gpsNewFrameUBLOX
  • 3.2 onGpsNewData
  • 3.3 GPS_calculateDistanceAndDirectionToHome
  • 3.4 GPS_calculateDistanceFlownVerticalSpeed
  • 3.5 rescueNewGpsData
• 4. 总结
• 5. 参考资料

GPS主要根据天空卫星信号来计算当前空间三位坐标位置，而其复杂内部运算逻辑以及保密性，都被封装到GPS模块中，对外提供串行接口，通常提供以下协议：

1. 【支持】NMEA
2. 【支持】UBlox
3. 【暂不支持】MSP: 该同等地位协议(GPS_STATE_INITIALIZED后，无实现)，猜测通过USB串口这通信线路与伴机电脑互联。
4. 【暂不支持】RTCM

因此，其硬件规格不像gyro/mag/baro等具体到硬件，更多是协议层面的支持。

根据BetaFlight深入传感设计：传感模块设计框架，我们针对如下几个阶段进行分析。

1. HwPreInit/HwInit阶段

1.1 【业务HwPreInit】gpsInit

GPS模块采用串口通信，因此该部分主要根据配置情况，初始化串口，并将GPS的状态机标记正确的位置GPS_STATE_INITIALIZING。

main
 └──> init
     └──> gpsInit

gpsInit
 ├──> [gpsData/gpsPacketLog结构体初始化]
 ├──> gpsSetState(GPS_STATE_UNKNOWN)
 ├──> <gpsConfig()->provider == GPS_MSP> // no serial ports used when GPS_MSP is configured
 │   ├──> gpsSetState(GPS_STATE_INITIALIZED)
 │   └──> return
 ├──> const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
 ├──> <!gpsPortConfig>
 │   └──> return  // 是否有外接GPS传感设备，如果没有则无相关应用
 ├──> <while (gpsInitData[gpsData.baudrateIndex].baudrateIndex != gpsPortConfig->gps_baudrateIndex>
 │   ├──> gpsData.baudrateIndex++
 │   └──> <gpsData.baudrateIndex >= GPS_INIT_DATA_ENTRY_COUNT>
 │       ├──> gpsData.baudrateIndex = DEFAULT_BAUD_RATE_INDEX
 │       └──> break
 ├──> portMode_e mode = MODE_RXTX
 ├──> <defined(GPS_NMEA_TX_ONLY)>
 │   └──> <gpsConfig()->provider == GPS_NMEA>
 │       └──> mode &= ~MODE_TX
 ├──> gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, NULL, baudRates[gpsInitData[gpsData.baudrateIndex].baudrateIndex], mode, SERIAL_NOT_INVERTED)
 ├──> <!gpsPort>
 │   └──> return
 └──> gpsSetState(GPS_STATE_INITIALIZING);  // signal GPS "thread" to initialize when it gets to it

1.2 【业务HwInit】gpsInitHardware

对硬件模块配置的通信协议，进行初始化：

1. NMEA
2. UBLox

注：传感模块协议初始化检查在gpsUpdate例程中的GPS_STATE_INITIALIZING/GPS_STATE_CHANGE_BAUD/GPS_STATE_CONFIGURE状态是调用。

gpsInitHardware
 └──> switch (gpsConfig()->provider) 
     ├──> case GPS_NMEA:
     │   ├──> gpsInitNmea()
     │   └──> break
     └──> case GPS_UBLOX:
         ├──> gpsInitUblox()
         └──> break

2. HwIo阶段

该阶段主要是间歇性读取并解析串行数据。

• 【数据接收】gpsNewData //串行数据一个一个字节输入，在协议面会进行状态机处理
• 【协议解析】gpsNewFrame

【NMEA】gpsNewFrameNMEA
【UBLOX】gpsNewFrameUBLOX

• 【数据处理】onGpsNewData

详见：【BetaFlight模块设计之八：GPS任务分析】

gpsUpdate
 ├──> <GPS data received via MSP>
 │   ├──> gpsSetState(GPS_STATE_RECEIVING_DATA)
 │   └──> onGpsNewData()
 ├──> <GPS data received via Serial port, NMEA or UBlox>
 │   ├──> <while (serialRxBytesWaiting(gpsPort))>
 │   │   ├──> <cmpTimeUs(micros(), currentTimeUs) > GPS_MAX_WAIT_DATA_RX)>  
 │   │   │   ├──> rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE_FAST))  // Wait 1ms and come back (1000Hz)
 │   │   │   └──> return
 │   │   └──> gpsNewData(serialRead(gpsPort))
 │   └──> rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_GPS_RATE))  // Restore default task rate(100Hz)
 ├──> [GPS 状态机]
 │   ├──> <GPS_STATE_UNKNOWN><GPS_STATE_INITIALIZED>
 │   │   └──> // do nothing, currently
 │   ├──> <GPS_STATE_INITIALIZING><GPS_STATE_CHANGE_BAUD><GPS_STATE_CONFIGURE>
 │   │   └──> gpsInitHardware()
 │   ├──> <GPS_STATE_LOST_COMMUNICATION>
 │   │   ├──> gpsData.timeouts++
 │   │   ├──> <gpsConfig()->autoBaud>
 │   │   │   ├──> gpsData.baudrateIndex++    // try another rate
 │   │   │   └──> gpsData.baudrateIndex %= GPS_INIT_ENTRIES
 │   │   ├──> gpsSol.numSat = 0
 │   │   ├──> DISABLE_STATE(GPS_FIX)
 │   │   └──> gpsSetState(GPS_STATE_INITIALIZING) 
 │   └──> <GPS_STATE_RECEIVING_DATA>  // check for no data/gps timeout/cable disconnection etc
 │       ├──> <millis() - gpsData.lastMessage > GPS_TIMEOUT>
 │       │   └──> gpsSetState(GPS_STATE_LOST_COMMUNICATION)
 │       └──> <USE_GPS_UBLOX><gpsConfig()->autoConfig == GPS_AUTOCONFIG_ON>
 │           ├──> <gpsData.state_position:0>
 │           │   └──> <!isConfiguratorConnected()>
 │           │       ├──> <gpsData.ubloxUseSAT>
 │           │       │   └──> ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 0) // disable SAT MSG
 │           │       ├──> <else>
 │           │       │   └──> ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 0) // disable SVINFO MSG
 │           │       └──> gpsData.state_position = 1
 │           ├──> <gpsData.state_position:1>
 │           │   ├──> <STATE(GPS_FIX) && (gpsConfig()->gps_ublox_mode == UBLOX_DYNAMIC)>
 │           │   │   ├──> ubloxSendNAV5Message(true)
 │           │   │   └──> gpsData.state_position = 2
 │           │   └──> <isConfiguratorConnected()>
 │           │       └──> gpsData.state_position = 2
 │           └──> <gpsData.state_position:2>
 │               └──> <isConfiguratorConnected()>
 │                   ├──> <gpsData.ubloxUseSAT>
 │                   │   └──> ubloxSetMessageRate(CLASS_NAV, MSG_SAT, 5) // set SAT MSG rate (every 5 cycles)
 │                   ├──> <else>
 │                   │   └──> ubloxSetMessageRate(CLASS_NAV, MSG_SVINFO, 5) // set SVINFO MSG rate (every 5 cycles)
 │                   └──> gpsData.state_position = 0
 ├──> executeTimeUs = micros() - currentTimeUs
 ├──> <executeTimeUs > gpsStateDurationUs[gpsCurrentState]>
 │   └──> gpsStateDurationUs[gpsCurrentState] = executeTimeUs
 ├──> schedulerSetNextStateTime(gpsStateDurationUs[gpsData.state])
 ├──> <sensors(SENSOR_GPS)>
 │   └──> updateGpsIndicator(currentTimeUs)
 ├──> <!ARMING_FLAG(ARMED) && !gpsConfig()->gps_set_home_point_once>
 │   └──> DISABLE_STATE(GPS_FIX_HOME)
 ├──> uint8_t minSats = 5
 ├──> <defined(USE_GPS_RESCUE)><gpsRescueIsConfigured()>
 │   ├──> updateGPSRescueState()
 │   └──> minSats = gpsRescueConfig()->minSats
 ├──> static bool hasFix = false
 ├──> <STATE(GPS_FIX)><gpsIsHealthy() && gpsSol.numSat >= minSats && !hasFix>  // ready beep sequence on fix or requirements for gps rescue met.    
 │   ├──> beeper(BEEPER_READY_BEEP)
 │   └──> hasFix = true
 └──> <else>
     └──> hasFix = false

3. HwDataAnalysis阶段

3.1 gpsNewFrame

目前GPS串行数据解析支持NMEA和UBLox两种帧格式，从协议的效率角度看UBLox的UBX协议会更加高效和精简。

注：目前BetaFlight固件推荐的也是UBLox协议配置。

3.1.1 【NMEA】gpsNewFrameNMEA

NMEA主要解析三种数据帧：

1. GPGGA //GPS定位信息，经纬度
2. GPRMC //推荐最小定位信息, 3d速度，方向角
3. GPGSV //可见卫星信息

gpsNewFrameNMEA
 ├──> [Frame Head checking]
 │   ├──> [GPGGA FRAME parsing]
 │   ├──> [GPRMC FRAME parsing]
 │   └──> [GPGSV FRAME parsing]
 └──> [checksum check and data applying]

注：记得刚毕业参加工作那会儿就自己根据NMEA写过一个解析函数，所以这块相对熟悉。后面给出字符串的例子。

例：$GPGGA,092204.999,4250.5589,S,14718.5084,E,1,04,24.4,19.7,M,,,,0000\ast 1F字段0：$GPGGA，语句ID，表明该语句为Global Positioning System Fix Data（GGA）GPS定位信息
字段1：UTC 时间，hhmmss.sss，时分秒格式
字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
字段3：纬度N（北纬）或S（南纬）
字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
字段5：经度E（东经）或W（西经）
字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
字段7：正在使用的卫星数量（00 - 12）（前导位数不足则补0）
字段8：HDOP水平精度因子（0.5 - 99.9）
字段9：海拔高度（-9999.9 - 99999.9）
字段10：地球椭球面相对大地水准面的高度
字段11：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空）
字段12：差分站ID号0000 - 1023（前导位数不足则补0，如果不是差分定位将为空）
字段13：校验值

例：$GPRMC,024813.640,A,3158.4608,N,11848.3737,E,10.05,324.27,150706,,,A\ast 50字段0：$GPRMC，语句ID，表明该语句为Recommended Minimum Specific GPS/TRANSIT Data（RMC）推荐最小定位信息
字段1：UTC时间，hhmmss.sss格式
字段2：状态，A=定位，V=未定位
字段3：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
字段4：纬度N（北纬）或S（南纬）
字段5：经度dddmm.mmmm，度分格式（前导位数不足则补0）
字段6：经度E（东经）或W（西经）
字段7：速度，节，Knots
字段8：方位角，度
字段9：UTC日期，DDMMYY格式
字段10：磁偏角，（000 - 180）度（前导位数不足则补0）
字段11：磁偏角方向，E=东W=西
字段16：校验值

例：$GPGSV,3,1,10,20,78,331,45,01,59,235,47,22,41,069,,13,32,252,45\ast 70字段0：$GPGSV，语句ID，表明该语句为GPS Satellites in View（GSV）可见卫星信息
字段1：本次GSV语句的总数目（1 - 3）
字段2：本条GSV语句是本次GSV语句的第几条（1 - 3）
字段3：当前可见卫星总数（00 - 12）（前导位数不足则补0）
字段4：PRN 码（伪随机噪声码）（01 - 32）（前导位数不足则补0）
字段5：卫星仰角（00 - 90）度（前导位数不足则补0）
字段6：卫星方位角（00 - 359）度（前导位数不足则补0）
字段7：信噪比（00－99）dbHz
字段8：PRN 码（伪随机噪声码）（01 - 32）（前导位数不足则补0）
字段9：卫星仰角（00 - 90）度（前导位数不足则补0）
字段10：卫星方位角（00 - 359）度（前导位数不足则补0）
字段11：信噪比（00－99）dbHz
字段12：PRN 码（伪随机噪声码）（01 - 32）（前导位数不足则补0）
字段13：卫星仰角（00 - 90）度（前导位数不足则补0）
字段14：卫星方位角（00 - 359）度（前导位数不足则补0）
字段15：信噪比（00－99）dbHz
字段16：校验值

3.1.2 【UBLOX】gpsNewFrameUBLOX

UBLox主要解析部分消息类型，如下所示：

• UBX-ACK (0x05)消息

ACK_ACK //UBX-ACK-ACK (0x05 0x01), Message acknowledged
ACK_NACK //UBX-ACK-NAK (0x05 0x00), Message not acknowledged

• UBX-NAV (0x01)消息

POSLLH //UBX-NAV-POSLLH (0x01 0x02), Geodetic position solution
STATUS //UBX-NAV-STATUS (0x01 0x03), Receiver navigation status
SOL //UBX-NAV-SOL (0x01 0x06), Navigation solution information
VELNED //UBX-NAV-VELNED (0x01 0x12), Velocity solution in NED frame
PVT //UBX-NAV-PVT (0x01 0x07), Navigation position velocity time solution
SVINFO //UBX-NAV-SVINFO (0x01 0x30), Space vehicle information
SAT //NAV-SAT, Satellite information

• 配置类消息

CFG_GNSS //GNSS system configuration

gpsNewFrameUBLOX
 ├──> [UBLOX Protocl Frame check]
 └──> UBLOX_parse_gps
     ├──> <MSG_POSLLH>
     ├──> <MSG_STATUS>
     ├──> <MSG_SOL>
     ├──> <MSG_VELNED>
     ├──> <MSG_PVT>
     ├──> <MSG_SVINFO>
     ├──> <MSG_SAT>
     ├──> <MSG_CFG_GNSS>
     ├──> <MSG_ACK_ACK>
     └──> <MSG_ACK_NACK>

3.2 onGpsNewData

当获取到最新数据是，计算如下业务数据：

1. 离开起飞点的距离
2. 更新飞行速度和方向
3. 标记新GPS数据，提示Resucre例程

onGpsNewData
 ├──> <!(STATE(GPS_FIX) && gpsSol.numSat >= 5)>
 │   └──> return;
 ├──> [Calculate time delta for navigation loop, range 0-1.0f, in seconds]
 │   ├──> static uint32_t nav_loopTimer;
 │   ├──> dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
 │   ├──> nav_loopTimer = millis();
 │   └──> dTnav = MIN(dTnav, 1.0f);  // prevent runup from bad GPS
 ├──> GPS_calculateDistanceAndDirectionToHome();
 ├──> <ARMING_FLAG(ARMED)>
 │   └──> GPS_calculateDistanceFlownVerticalSpeed(false);
 └──> <USE_GPS_RESCUE>
     └──> rescueNewGpsData();

3.3 GPS_calculateDistanceAndDirectionToHome

计算水平方向距离起飞点距离，不考虑高度。

void GPS_calculateDistanceAndDirectionToHome(void)

    if (STATE(GPS_FIX_HOME))       // If we don't have home set, do not display anything
        uint32_t dist;
        int32_t dir;
        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
        GPS_distanceToHome = dist / 100;
        GPS_directionToHome = dir / 100;
     else 
        GPS_distanceToHome = 0;
        GPS_directionToHome = 0;
    

#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)

    float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
    float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
    *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;

    *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
    if (*bearing < 0)
        *bearing += 36000;

3.4 GPS_calculateDistanceFlownVerticalSpeed

计算飞行速度和方向。

static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)

    static int32_t lastCoord[2] =  0, 0 ;
    static int32_t lastAlt;
    static int32_t lastMillis;

    int currentMillis = millis();

    if (initialize) 
        GPS_distanceFlownInCm = 0;
        GPS_verticalSpeedInCmS = 0;
     else 
        if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) 
            uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
            // Only add up movement when speed is faster than minimum threshold
            if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) 
                uint32_t dist;
                int32_t dir;
                GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
                if (gpsConfig()->gps_use_3d_speed) 
                    dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
                
                GPS_distanceFlownInCm += dist;
            
        
        GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
        GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
    
    lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
    lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
    lastAlt = gpsSol.llh.altCm;
    lastMillis = currentMillis;

3.5 rescueNewGpsData

/*
 If we have new GPS data, update home heading
 if possible and applicable.
*/
void rescueNewGpsData(void)

    newGPSData = true;

4. 总结

GPS传感器通过NMEA和UBLox协议解析获取数据，其业务飞行器速度，飞行方向，以及距离起飞点水平距离和方向角的计算都容易理解，且不复杂。

5. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight模块设计之八：GPS任务分析
【3】Wiki, NMEA_0183
【4】u-blox8-M8-ReceiverDescrProtSpec-UBX-13003221