Sending GPS to the FC

[miguelwon]

Hi, I'm trying to send GPS data through MSP to an FC with betaflight (the current latest version). I'm using code MSP_SET_RAW_GPS that according to this it sets a set of GPS observables. I show below an example of a code I'm using that sends PS4 controller commands for roll, pitch, yaw, and throttle, followed by an attempt to send the GPS data as well. My problem is that the GPS section is not working. For some reason, it freezes the loop. Any idea why is not working? Note that I'm using betaflight because it allows setting MSP GPS mode, which I believe is to work precisely as I'm trying to do.

import time
from yamspy import MSPy
from approxeng.input.selectbinder import ControllerResource

serial_port = "/dev/serial0"
FC_SEND_LOOP_TIME = 0.05
CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2']
CMDS = {
        'roll':     1500,
        'pitch':    1500,
        'yaw':      1500,
        'aux1':     1000,
        'aux2':     1000
        }

def stick2pwm(x):
    return int(200*x)

with MSPy(device=serial_port, loglevel='WARNING', baudrate=115200) as board:
    command_list = ['MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION', 'MSP_BUILD_INFO',
                    'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM', 'MSP_NAME', 'MSP_STATUS',
                    'MSP_STATUS_EX','MSP_BATTERY_CONFIG', 'MSP_BATTERY_STATE', 'MSP_BOXNAMES']
    for msg in command_list:
        if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
            dataHandler = board.receive_msg()
            board.process_recv_data(dataHandler)
    try:

        with ControllerResource() as joystick:
            while joystick.connected:
                #lx -> ga_0
                #ly -> ga_1

                #rx -> ga_2
                #ry -> ga_3
                lx,ly,rx,ry,triangle,rt,lt = joystick['lx','ly','rx','ry','triangle','rt','lt']
                throttle = ly
                yaw = lx
                roll = rx
                pitch = ry

                if throttle > 0:
                    CMDS['throttle'] = 1000+stick2pwm(throttle)
                else:
                    CMDS['throttle'] = 1000

                if yaw > 0:
                    CMDS['yaw'] = 1500+stick2pwm(yaw)
                elif yaw < 0:
                    CMDS['yaw'] = 1500+stick2pwm(yaw)
                else:
                    CMDS['yaw'] = 1500

                if roll > 0:
                    CMDS['roll'] = 1500+stick2pwm(roll)
                elif roll < 0:
                    CMDS['roll'] = 1500+stick2pwm(roll)
                else:
                    CMDS['roll'] = 1500

                if pitch > 0:
                    CMDS['pitch'] = 1500+stick2pwm(pitch)
                elif pitch < 0:
                    CMDS['pitch'] = 1500+stick2pwm(pitch)
                else:
                    CMDS['pitch'] = 1500

                if rt > 0.9 and CMDS['aux1'] < 1500:
                    CMDS['aux1'] = 1800
                elif lt > 0.9 and CMDS['aux1'] > 1500:
                    CMDS['aux1'] = 1000

                #send cmds to FC
                CMDS_RC = [CMDS[ki] for ki in CMDS_ORDER]
                if board.send_RAW_RC(CMDS_RC):
                    dataHandler = board.receive_msg()
                    board.process_recv_data(dataHandler)

                ############ SEND GPS DATA #############
                gps_fix = 1
                gps_numSat = 7 #num_sat
                gps_lat = int(3843.037537*10000000) #lat
                gps_lon = int(00907.745287*10000000) #lon
                gps_alt = int(89.2*100) #altitude
                gps_spe = int(0.0) #spd_over_grnd
                gps_grd_cou = int(74.6*10) #true_course
                # gps_hdop = int(1.0*100)

                data_gps = board.convert([gps_fix], 8)
                data_gps += board.convert([gps_numSat], 8)
                data_gps += board.convert([gps_lat], 32)
                data_gps += board.convert([gps_lon], 32)
                data_gps += board.convert([gps_alt], 16)
                data_gps += board.convert([gps_spe], 16)
                # data_gps += board.convert([gps_grd_cou], 16)

                if board.send_RAW_msg(MSPy.MSPCodes['MSP_SET_RAW_GPS'], data=data_gps):
                    dataHandler = board.receive_msg()
                    board.process_recv_data(dataHandler)

                #######################################

                time.sleep(FC_SEND_LOOP_TIME)

    except KeyboardInterrupt:
        print("stop")
        CMDS = {
                'roll':     1500,
                'pitch':    1500,
                'yaw':      1500,
                'aux1':     1000,
                'aux2':     1000
                }
        CMDS_RC = [CMDS[ki] for ki in CMDS_ORDER]
        if board.send_RAW_RC(CMDS_RC):
            dataHandler = board.receive_msg()
            board.process_recv_data(dataHandler)

        pass

[ricardodeazambuja]

Could you check where it is blocking? It will help to know whether it blocks on board.send_RAW_msg(MSPy.MSPCodes['MSP_SET_RAW_GPS'], data=data_gps), dataHandler = board.receive_msg() or board.process_recv_data(dataHandler)

[ricardodeazambuja]

Ok, I was curious and I flashed BF 4.2.9 on the FC I have here and the code below seems to work as expected (checks GPS, gets nothing, sends GPS, gets the GPS data sent before).

import time
from yamspy import MSPy

serial_port = "/dev/ttyACM0"
FC_SEND_LOOP_TIME = 0.05
CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2']
CMDS = {
        'roll':     1500,
        'pitch':    1500,
        'yaw':      1500,
        'aux1':     1000,
        'aux2':     1000
        }

with MSPy(device=serial_port, loglevel='WARNING', baudrate=115200) as board:
    command_list = ['MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION', 'MSP_BUILD_INFO',
                    'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM', 'MSP_NAME', 'MSP_STATUS',
                    'MSP_STATUS_EX','MSP_BATTERY_CONFIG', 'MSP_BATTERY_STATE', 'MSP_BOXNAMES']
    for msg in command_list:
        if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]):
            dataHandler = board.receive_msg()
            board.process_recv_data(dataHandler)
    try:
        ############ SEND GPS DATA #############
        gps_fix = 1
        gps_numSat = 7 #num_sat
        gps_lat = int(3843.037537*10000000)
        gps_lon = int(00907.745287*10000000) #lon
        gps_alt = int(89.2*100) #altitude
        gps_spe = int(0.0) #spd_over_grnd
        gps_grd_cou = int(74.6*10) #true_course

        data_gps = board.convert([gps_fix], 8)
        data_gps += board.convert([gps_numSat], 8)
        data_gps += board.convert([gps_lat], 32)
        data_gps += board.convert([gps_lon], 32)
        data_gps += board.convert([gps_alt], 16)
        data_gps += board.convert([gps_spe], 16)

        # Ask GPS data
        if board.send_RAW_msg(MSPy.MSPCodes['MSP_RAW_GPS'], data=data_gps):
            print("data sent!")
            dataHandler = board.receive_msg()
            print("data received!")
            board.process_recv_data(dataHandler)
            print("data processed!")

        # Received GPS data
        print(board.GPS_DATA)

        # Send GPS data
        if board.send_RAW_msg(MSPy.MSPCodes['MSP_SET_RAW_GPS'], data=data_gps):
            print("data sent!")
            dataHandler = board.receive_msg()
            print("data received!")
            board.process_recv_data(dataHandler)
            print("data processed!")

        # Ask GPS data
        if board.send_RAW_msg(MSPy.MSPCodes['MSP_RAW_GPS'], data=data_gps):
            print("data sent!")
            dataHandler = board.receive_msg()
            print("data received!")
            board.process_recv_data(dataHandler)
            print("data processed!")

        # Received GPS data
        print(board.GPS_DATA)

        time.sleep(FC_SEND_LOOP_TIME)

    except KeyboardInterrupt:
        print("stop")
    finally:
        board.reboot()

[miguelwon]

Ok, nice! Let me try it.

[ricardodeazambuja]

Actually, some of the values received are not correct:

{'fix': 2, 'numSat': 7, 'lat': -224330294, 'lon': 487518278, 'alt': 8920, 'speed': 0, 'ground_course': 0, 'distanceToHome': 0, 'ditectionToHome': 0, 'update': 0, 'chn': [], 'svid': [], 'quality': [], 'cno': []}

[ricardodeazambuja]

Ok, the only field that is problematic is 'fix'.

[ricardodeazambuja]

According to runtime_config.h, gps.c, and msp.c, the GPS_DATA['fix'] is giving the correct value:

typedef enum {
    GPS_FIX_HOME   = (1 << 0),
    GPS_FIX        = (1 << 1),
    GPS_FIX_EVER   = (1 << 2),
} stateFlags_t;

void gpsSetFixState(bool state)
{
    if (state) {
        ENABLE_STATE(GPS_FIX);
        ENABLE_STATE(GPS_FIX_EVER);
    } else {
        DISABLE_STATE(GPS_FIX);
    }
}

    case MSP_RAW_GPS:
        sbufWriteU8(dst, STATE(GPS_FIX));
        sbufWriteU8(dst, gpsSol.numSat);
        sbufWriteU32(dst, gpsSol.llh.lat);
        sbufWriteU32(dst, gpsSol.llh.lon);
        sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
        sbufWriteU16(dst, gpsSol.groundSpeed);
        sbufWriteU16(dst, gpsSol.groundCourse);
        // Added in API version 1.44    
        sbufWriteU16(dst, gpsSol.hdop);
        break;

    case MSP_SET_RAW_GPS:
        gpsSetFixState(sbufReadU8(src));
        gpsSol.numSat = sbufReadU8(src);
        gpsSol.llh.lat = sbufReadU32(src);
        gpsSol.llh.lon = sbufReadU32(src);
        gpsSol.llh.altCm = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
        gpsSol.groundSpeed = sbufReadU16(src);
        GPS_update |= GPS_MSP_UPDATE;        // MSP data signalisation to GPS functions
        break;

[miguelwon]

Just found that my problem was related to the fact I had GPS configured as MSP, as I think is supposed to be. If I turn it off I get your results. But even so, the betaflight configurator is not showing GPS lat and log values.

[ricardodeazambuja]

I set GPS to MSP, just like you did, and the same code I posted here still works. I can't send GPS MSP messages while using the configurator because I am connecting using the USB, so I can't connect to two things at the same time.

[miguelwon]

I'm using MSP through UART. The problem was related to Auto Config. For some reason, it messes with the communication.
Anyway, I couldn't figure out why it's not working but found that INAV works fine. So, will move to INAV... Thanks!