STM32 GPS悬停飞控 （三十六）树莓派 数传串口解码

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接： https://blog.csdn.net/shukebeta008/article/details/101645875

我写了个串口接收程序，然后仿照c#地面站写了解码的程序，测试下来可以用。

我测试时飞控只用了核心板，把飞控里好多检查的代码去掉才会有数传数据发出来。然后我把cp2102 rx接到数传口上，cp2102另一头是接在ubuntu电脑上跑的。跑下来没问题后可以放到树莓派飞控上试试看。

接下来还要写一个socket转发程序。转发串口数据再解码。

#!/usr/bin/python
# coding=UTF-8

import serial

received_data = 0
receive_buffer_counter = 0
receive_start_detect = 0
receive_buffer = [0 for i in range(50)]
hex_buffer = [0 for i in range(50)]
receive_byte_previous = 0
max_altitude_meters = 0

def hexshow(data):
    e = 0
    for i in data:
        d = ord(i)
        e = e*256 + d
    return e

def get_data():
    global max_altitude_meters
    for i in range(34):
        hex_buffer[i] = hexshow(receive_buffer[i])
    print hex_buffer
    print receive_buffer

    check_byte = 0;
    for temp_byte in range(30):
        check_byte ^= hex_buffer[temp_byte]

    if check_byte == hex_buffer[31]:
        error = hex_buffer[0]
        flight_mode = hex_buffer[1]
        battery_voltage = (float)(hex_buffer[2])/10.0
        temperature = hex_buffer[3] | ( hex_buffer[4]<<8)
        roll_angle = hex_buffer[5] - 100
        pitch_angle = hex_buffer[6] - 100
        start = hex_buffer[7]
        altitude_meters = (hex_buffer[8] | hex_buffer[9] << 8) - 1000
        if altitude_meters > max_altitude_meters:
            max_altitude_meters = altitude_meters
        takeoff_throttle = hex_buffer[10] | hex_buffer[11] << 8
        actual_compass_heading = hex_buffer[12] | hex_buffer[13] << 8
        heading_lock = hex_buffer[14]
        number_used_sats = hex_buffer[15]
        fix_type = hex_buffer[16]
        l_lat_gps = hex_buffer[17] | hex_buffer[18] << 8 | hex_buffer[19] << 16 | hex_buffer[20] << 24
        l_lon_gps = hex_buffer[21] | hex_buffer[22] << 8 | hex_buffer[23] << 16 | hex_buffer[24] << 24
        adjustable_setting_1 = (float)(hex_buffer[25] | hex_buffer[26] << 8) / 100.0;
        adjustable_setting_2 = (float)(hex_buffer[27] | hex_buffer[28] << 8) / 100.0;
        adjustable_setting_3 = (float)(hex_buffer[29] | hex_buffer[30] << 8) / 100.0;
        print error, flight_mode, battery_voltage, temperature, roll_angle, pitch_angle, start, altitude_meters, takeoff_throttle   
        print actual_compass_heading, heading_lock, number_used_sats, fix_type, l_lat_gps, l_lon_gps, adjustable_setting_1, adjustable_setting_2, adjustable_setting_3

if __name__ == '__main__':
    serial = serial.Serial('/dev/ttyUSB0', 9600)
    print serial
    if serial.isOpen():
       print("open success")
    else:
        print("open failed")
    try:
        while True:
            count = serial.inWaiting()
            if count > 0:
                if received_data == 0:
                    received_data = 1
                nextByte = serial.read(count)
                if nextByte >= 0:
                    receive_buffer[receive_buffer_counter] = nextByte     
                if receive_byte_previous == 'J' and receive_buffer[receive_buffer_counter] == 'B':
                    receive_buffer_counter = 0
                    receive_start_detect = receive_start_detect + 1
                    if receive_start_detect >= 2:
                        get_data()
                else:
                    receive_byte_previous = receive_buffer[receive_buffer_counter]
                    receive_buffer_counter = receive_buffer_counter + 1
                    if (receive_buffer_counter > 48):
                        receive_buffer_counter = 0             
    except KeyboardInterrupt:
        if serial != None:
            serial.close()