コード例 #1
0
def tick(t, data):
    global LAST_CONTROL_DATA

    state_pos = None
    state_velocity = (-1, -1, -1)
    state_wind = (0, 0, 0)
    state_attitude = (0, 0, 0)
    state_angular_velocity = (0, 0, 0)
    control_refp = None
    control_path = -1
    control_cycles = -1
    control_obj_val = -1
    control_errors = -1
    control_resets = -1
    control_values = (-1, -1, -1)
    control_mode = [-1, -1]
    control_nav_version = -1
    control_pos = (-1, -1, -1)
    control_error_type = 0
    last_gps = 0
    last_data = 0
    payload_release = False

    for param in data:
        pt = param.parameter_type
        if isinstance(param, plog.DataParameter):
            pv = param.values
        else:
            pv = param.value

        if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
            state_pos = (pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31,
                         pv[2] * 1e-2)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
            state_velocity = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
            att_q = map(lambda x: float(x) / 32767.0, pv)
            state_attitude = plog.q_to_euler(att_q)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ:
            state_angular_velocity = map(
                lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)),
                pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
            state_wind = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE:
            mode = chr(pv[0])
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE:
            control_mode[param.device_id] = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT:
            control_values = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS and param.device_id == 1:
            control_pos = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID:
            control_path = pv[0]
            control_error_type = pv[1] if len(pv) > 1 else 0
        elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
            control_refp = plog.extract_waypoint(pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS:
            control_cycles = pv[0]
            control_obj_val = pv[1]
            control_errors = pv[2]
            control_resets = pv[3]
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS:
            last_gps = pv[0]
            last_data = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION:
            control_nav_version = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_GP_OUT:
            payload_release = True if pv[0] else False

    control_data = (control_cycles, control_obj_val, control_errors,
                    control_resets)
    if not state_pos or not control_refp or LAST_CONTROL_DATA == control_data:
        return ""
    #if control_mode[1] != 1:
    #    return ""

    LAST_CONTROL_DATA = control_data

    state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 +
                               (state_velocity[1] - state_wind[1])**2 +
                               (state_velocity[2] - state_wind[2])**2)
    state_groundspeed = math.sqrt(state_velocity[0]**2 + state_velocity[1]**2)

    ned_off = plog.lla_to_ned(
        state_pos,
        (control_refp["lat"], control_refp["lon"], control_refp["alt"]))
    ref_v = (control_refp["airspeed"] * math.cos(control_refp["yaw"]) +
             state_wind[0],
             control_refp["airspeed"] * math.sin(control_refp["yaw"]) +
             state_wind[1])
    ate = abs((ned_off[0] * ref_v[0] + ned_off[1] * ref_v[1]) / \
              math.sqrt(ref_v[0] ** 2 + ref_v[1] ** 2))
    xte = math.sqrt((ned_off[0]**2 + ned_off[1]**2) - ate**2)

    data = ((t, ) + (state_pos[2], control_refp["alt"]) + ned_off +
            (xte, ate) +
            (state_airspeed, control_refp["airspeed"], state_groundspeed) +
            (math.degrees(math.atan2(state_velocity[1], state_velocity[0])),
             math.degrees(math.atan2(ref_v[1], ref_v[0]))) +
            (state_attitude[0], math.degrees(control_refp["yaw"])) +
            (state_attitude[1], math.degrees(control_refp["pitch"])) +
            (state_attitude[2], math.degrees(control_refp["roll"])) +
            (state_angular_velocity[2], state_angular_velocity[1],
             state_angular_velocity[0]) +
            (math.sqrt(state_wind[0]**2 + state_wind[1]**2), state_wind[0],
             state_wind[1]) +
            (control_values[0], control_values[1], control_values[2]) +
            (control_pos[0], control_pos[1], control_pos[2]) +
            (control_obj_val, control_cycles, control_errors, control_resets) +
            (control_path, last_gps, control_mode[1], control_refp["flags"],
             control_error_type, 1 if payload_release else 0))

    format = (
        "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " +
        "n=%6.2f, e=%6.2f, d=%6.2f, xte=%4.2f, ate=%4.2f, " +
        "tas=%5.2f, tas_ref=%5.2f, gs=%5.2f, " +
        "heading=%3.0f, heading_ref=%3.0f, " + "yaw=%3.0f, yaw_ref=%3.0f, " +
        "pitch=%4.0f, pitch_ref=%4.0f, " + "roll=%4.0f, roll_ref=%4.0f, " +
        "vyaw=%4.0f, vpitch=%4.0f, vroll=%4.0f, " +
        "wind=%5.2f, wind_n=%5.2f, wind_e=%5.2f, " +
        "t=%.3f, l=%.3f, r=%.3f, " + "tp=%.3f, lp=%.3f, rp=%.3f, " +
        "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " +
        "path=%4d, last_gps=%4d, mode1=%d, flags=%08x, err=%04x, release=%d\n")

    dump = ("%d," + "%.3f,%.3f," + "%.2f,%.2f,%.2f,%.2f,%.2f," +
            "%.2f,%.2f,%.2f," + "%.1f,%.1f," + "%.1f,%.1f," + "%.1f,%.1f," +
            "%.1f,%.1f," + "%.1f,%.1f,%.1f," + "%.2f,%.2f,%.2f," +
            "%.3f,%.3f,%.3f," + "%.3f,%.3f,%.3f," + "%.1f,%d,%d,%d," +
            "%d,%d,%d,%d,%d,%d\n")

    #return format % data
    return dump % data
コード例 #2
0
ファイル: controllog.py プロジェクト: liubingzsd/fcs
def tick(t, data):
    global LAST_CONTROL_DATA

    state_pos = None
    state_velocity = (-1, -1, -1)
    state_wind = (0, 0, 0)
    state_attitude = (0, 0, 0)
    state_angular_velocity = (0, 0, 0)
    control_refp = None
    control_path = -1
    control_cycles = -1
    control_obj_val = -1
    control_errors = -1
    control_resets = -1
    control_values = (-1, -1, -1)
    control_mode = [-1, -1]
    control_nav_version = -1
    control_pos = (-1, -1, -1)
    control_error_type = 0
    last_gps = 0
    last_data = 0
    payload_release = False

    for param in data:
        pt = param.parameter_type
        if isinstance(param, plog.DataParameter):
            pv = param.values
        else:
            pv = param.value

        if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
            state_pos = (
                pv[0] * math.pi / 2**31,
                pv[1] * math.pi / 2**31,
                pv[2] * 1e-2
            )
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
            state_velocity = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
            att_q = map(lambda x: float(x) / 32767.0, pv)
            state_attitude = plog.q_to_euler(att_q)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ:
            state_angular_velocity = map(
                lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)),
                pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
            state_wind = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE:
            mode = chr(pv[0])
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE:
            control_mode[param.device_id] = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT:
            control_values = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS and param.device_id == 1:
            control_pos = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID:
            control_path = pv[0]
            control_error_type = pv[1] if len(pv) > 1 else 0
        elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
            control_refp = plog.extract_waypoint(pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS:
            control_cycles = pv[0]
            control_obj_val = pv[1]
            control_errors = pv[2]
            control_resets = pv[3]
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS:
            last_gps = pv[0]
            last_data = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION:
            control_nav_version = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_GP_OUT:
            payload_release = True if pv[0] else False


    control_data = (control_cycles, control_obj_val, control_errors,
                    control_resets)
    if not state_pos or not control_refp or LAST_CONTROL_DATA == control_data:
        return ""
    #if control_mode[1] != 1:
    #    return ""

    LAST_CONTROL_DATA = control_data

    state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 +
                               (state_velocity[1] - state_wind[1])**2 +
                               (state_velocity[2] - state_wind[2])**2)
    state_groundspeed = math.sqrt(state_velocity[0] ** 2 +
                                  state_velocity[1] ** 2)

    ned_off = plog.lla_to_ned(
        state_pos,
        (control_refp["lat"], control_refp["lon"], control_refp["alt"]))
    ref_v = (
        control_refp["airspeed"] * math.cos(control_refp["yaw"]) + state_wind[0],
        control_refp["airspeed"] * math.sin(control_refp["yaw"]) + state_wind[1]
    )
    ate = abs((ned_off[0] * ref_v[0] + ned_off[1] * ref_v[1]) / \
              math.sqrt(ref_v[0] ** 2 + ref_v[1] ** 2))
    xte = math.sqrt((ned_off[0] ** 2 + ned_off[1] ** 2) - ate ** 2)

    data = (
        (t, ) +
        (state_pos[2], control_refp["alt"]) +
        ned_off + (xte, ate) +
        (state_airspeed, control_refp["airspeed"], state_groundspeed) +
        (math.degrees(math.atan2(state_velocity[1], state_velocity[0])),
            math.degrees(math.atan2(ref_v[1], ref_v[0]))) +
        (state_attitude[0], math.degrees(control_refp["yaw"])) +
        (state_attitude[1], math.degrees(control_refp["pitch"])) +
        (state_attitude[2], math.degrees(control_refp["roll"])) +
        (state_angular_velocity[2], state_angular_velocity[1],
            state_angular_velocity[0]) +
        (math.sqrt(state_wind[0] ** 2 + state_wind[1] ** 2), state_wind[0],
            state_wind[1]) +
        (control_values[0], control_values[1], control_values[2]) +
        (control_pos[0], control_pos[1], control_pos[2]) +
        (control_obj_val, control_cycles, control_errors, control_resets) +
        (control_path, last_gps, control_mode[1], control_refp["flags"],
            control_error_type, 1 if payload_release else 0)
    )

    format = (
        "t=%8d, " +
        "alt=%3.3f, alt_ref=%3.3f, " +
        "n=%6.2f, e=%6.2f, d=%6.2f, xte=%4.2f, ate=%4.2f, " +
        "tas=%5.2f, tas_ref=%5.2f, gs=%5.2f, " +
        "heading=%3.0f, heading_ref=%3.0f, " +
        "yaw=%3.0f, yaw_ref=%3.0f, " +
        "pitch=%4.0f, pitch_ref=%4.0f, " +
        "roll=%4.0f, roll_ref=%4.0f, " +
        "vyaw=%4.0f, vpitch=%4.0f, vroll=%4.0f, " +
        "wind=%5.2f, wind_n=%5.2f, wind_e=%5.2f, " +
        "t=%.3f, l=%.3f, r=%.3f, " +
        "tp=%.3f, lp=%.3f, rp=%.3f, " +
        "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " +
        "path=%4d, last_gps=%4d, mode1=%d, flags=%08x, err=%04x, release=%d\n"
    )

    dump = (
        "%d," +
        "%.3f,%.3f," +
        "%.2f,%.2f,%.2f,%.2f,%.2f," +
        "%.2f,%.2f,%.2f," +
        "%.1f,%.1f," +
        "%.1f,%.1f," +
        "%.1f,%.1f," +
        "%.1f,%.1f," +
        "%.1f,%.1f,%.1f," +
        "%.2f,%.2f,%.2f," +
        "%.3f,%.3f,%.3f," +
        "%.3f,%.3f,%.3f," +
        "%.1f,%d,%d,%d," +
        "%d,%d,%d,%d,%d,%d\n"
    )

    #return format % data
    return dump % data
コード例 #3
0
ファイル: kmlview.py プロジェクト: liubingzsd/fcs
                    pv = param.values
                else:
                    pv = param.value

                if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
                    point =  '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format(
                        # KML is lon, lat
                        math.degrees(pv[1] * math.pi / 2**31),
                        math.degrees(pv[0] * math.pi / 2**31),
                        pv[2] * 1e-2
                    )
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
                    velocity = map(lambda x: float(x) * 1e-2, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
                    att_q = map(lambda x: float(x) / 32767.0, pv)
                    attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format(*plog.q_to_euler(att_q))
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
                    wind = map(lambda x: float(x) * 1e-2, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS:
                    control_pos = map(lambda x: float(x) / 65535.0, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
                    ref_waypoint = plog.extract_waypoint(pv)
                    ref_point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format(
                        math.degrees(ref_waypoint['lon']),
                        math.degrees(ref_waypoint['lat']),
                        ref_waypoint['alt']
                    )
                    ref_attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format(
                        math.degrees(ref_waypoint['yaw']),
                        math.degrees(ref_waypoint['pitch']),
                        math.degrees(ref_waypoint['roll'])
コード例 #4
0
                    pv = param.values
                else:
                    pv = param.value

                if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
                    point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format(
                        # KML is lon, lat
                        math.degrees(pv[1] * math.pi / 2**31),
                        math.degrees(pv[0] * math.pi / 2**31),
                        pv[2] * 1e-2)
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
                    velocity = map(lambda x: float(x) * 1e-2, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
                    att_q = map(lambda x: float(x) / 32767.0, pv)
                    attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format(
                        *plog.q_to_euler(att_q))
                elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
                    wind = map(lambda x: float(x) * 1e-2, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_POS:
                    control_pos = map(lambda x: float(x) / 65535.0, pv)
                elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
                    ref_waypoint = plog.extract_waypoint(pv)
                    ref_point = '<gx:coord>{0} {1} {2:.2f}</gx:coord>'.format(
                        math.degrees(ref_waypoint['lon']),
                        math.degrees(ref_waypoint['lat']), ref_waypoint['alt'])
                    ref_attitude = '<gx:angles>{0:.1f} {1:.1f} {2:.1f}</gx:angles>'.format(
                        math.degrees(ref_waypoint['yaw']),
                        math.degrees(ref_waypoint['pitch']),
                        math.degrees(ref_waypoint['roll']))
                    ref_airspeed = '<gx:value>{0:.2f}</gx:value>'.format(
                        ref_waypoint['airspeed'])
コード例 #5
0
ファイル: gcsconsole.py プロジェクト: mfkiwl/fcs
def tick(t, data):
    global LAST_CONTROL_DATA

    state_pos = None
    state_velocity = (-1, -1, -1)
    state_wind = (0, 0, 0)
    state_attitude = (0, 0, 0)
    state_angular_velocity = (0, 0, 0)
    control_refp = None
    control_path = -1
    control_cycles = -1
    control_obj_val = -1
    control_errors = -1
    control_resets = -1
    control_values = (-1, -1, -1)
    control_mode = [-1, -1]
    control_nav_version = -1
    control_pos = (-1, -1, -1)
    control_error_type = 0
    last_gps = 0
    last_data = 0

    for param in data:
        pt = param.parameter_type
        if isinstance(param, plog.DataParameter):
            pv = param.values
        else:
            pv = param.value

        if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
            state_pos = (pv[0] * math.pi / 2**31, pv[1] * math.pi / 2**31,
                         pv[2] * 1e-2)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
            state_velocity = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
            att_q = map(lambda x: float(x) / 32767.0, pv)
            state_attitude = plog.q_to_euler(att_q)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
            state_wind = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE:
            mode = chr(pv[0])
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE:
            control_mode[param.device_id] = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT:
            control_values = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID:
            control_path = pv[0]
            control_error_type = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
            control_refp = plog.extract_waypoint(pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS:
            control_cycles = pv[0]
            control_obj_val = pv[1]
            control_errors = pv[2]
            control_resets = pv[3]
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS:
            last_gps = pv[0]
            last_data = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION:
            control_nav_version = pv[0]

    control_data = (control_cycles, control_obj_val, control_errors,
                    control_resets)
    if not state_pos or not control_refp:
        return ""

    LAST_CONTROL_DATA = control_data

    state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 +
                               (state_velocity[1] - state_wind[1])**2 +
                               (state_velocity[2] - state_wind[2])**2)

    return (
        "t=%8d, " + "alt=%3.3f, alt_ref=%3.3f, " +
        "n=%6.2f, e=%6.2f, d=%6.2f, " + "tas=%5.2f, tas_ref=%5.2f, " +
        "yaw=%3.0f, yaw_ref=%3.0f, " + "pitch=%4.0f, pitch_ref=%4.0f, " +
        "roll=%4.0f, roll_ref=%4.0f, " + "t=%.3f, l=%.3f, r=%.3f, " +
        "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " +
        "path=%4d, last_gps=%4d, mode1=%d, nav_state=%d") % (
            (t, ) +
            (state_pos[2], control_refp.get("alt", 0)) + plog.lla_to_ned(
                (control_refp.get("lat", 0), control_refp.get(
                    "lon", 0), control_refp.get("alt", 0)), state_pos) +
            (state_airspeed, control_refp.get("airspeed", 0)) +
            (state_attitude[0], math.degrees(control_refp.get("yaw", 0))) +
            (state_attitude[1], math.degrees(control_refp.get("pitch", 0))) +
            (state_attitude[2], math.degrees(control_refp.get("roll", 0))) +
            (control_values[0], control_values[1], control_values[2]) +
            (control_obj_val, control_cycles, control_errors, control_resets) +
            (control_path, last_gps, control_mode[1], control_nav_version))
コード例 #6
0
ファイル: estimate.py プロジェクト: mfkiwl/fcs
        estimate_t += 0.001

        for param in data:
            pt = param.parameter_type
            pv = param.values
            if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
                pos = [
                    math.degrees(pv[0] * math.pi / 2**31),
                    math.degrees(pv[1] * math.pi / 2**31),
                    pv[2] * 1e-2
                ]
            elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
                v = map(lambda x: float(x) * 1e-2, pv)
            elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
                att_q = map(lambda x: float(x) / 32767.0, pv)
                att_ypr = list(plog.q_to_euler(att_q))
            elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ:
                angular_v = map(
                    lambda x: math.degrees(float(x) / (32767.0 / math.pi * 0.25)),
                    pv)
            elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
                wind_v = map(lambda x: float(x) * 1e-2, pv)
            elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE:
                mode = chr(pv[0])
            elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_GYRO_BIAS_XYZ:
                gyro_bias = map(
                    lambda x: math.degrees(float(x) / (32767.0 / math.pi * 2.0)),
                    pv)

        vv = math.sqrt(v[0] ** 2 + v[1] ** 2 + v[2] ** 2)
コード例 #7
0
ファイル: gcsconsole.py プロジェクト: liubingzsd/fcs
def tick(t, data):
    global LAST_CONTROL_DATA

    state_pos = None
    state_velocity = (-1, -1, -1)
    state_wind = (0, 0, 0)
    state_attitude = (0, 0, 0)
    state_angular_velocity = (0, 0, 0)
    control_refp = None
    control_path = -1
    control_cycles = -1
    control_obj_val = -1
    control_errors = -1
    control_resets = -1
    control_values = (-1, -1, -1)
    control_mode = [-1, -1]
    control_nav_version = -1
    control_pos = (-1, -1, -1)
    control_error_type = 0
    last_gps = 0
    last_data = 0

    for param in data:
        pt = param.parameter_type
        if isinstance(param, plog.DataParameter):
            pv = param.values
        else:
            pv = param.value

        if pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA:
            state_pos = (
                pv[0] * math.pi / 2**31,
                pv[1] * math.pi / 2**31,
                pv[2] * 1e-2
            )
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED:
            state_velocity = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q:
            att_q = map(lambda x: float(x) / 32767.0, pv)
            state_attitude = plog.q_to_euler(att_q)
        elif pt == plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED:
            state_wind = map(lambda x: float(x) * 1e-2, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_MODE:
            mode = chr(pv[0])
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_MODE:
            control_mode[param.device_id] = pv[0]
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT:
            control_values = map(lambda x: float(x) / 65535.0, pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID:
            control_path = pv[0]
            control_error_type = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_KEY_VALUE:
            control_refp = plog.extract_waypoint(pv)
        elif pt == plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS:
            control_cycles = pv[0]
            control_obj_val = pv[1]
            control_errors = pv[2]
            control_resets = pv[3]
        elif pt == plog.ParameterType.FCS_PARAMETER_AHRS_STATUS:
            last_gps = pv[0]
            last_data = pv[1]
        elif pt == plog.ParameterType.FCS_PARAMETER_NAV_VERSION:
            control_nav_version = pv[0]


    control_data = (control_cycles, control_obj_val, control_errors,
                    control_resets)
    if not state_pos or not control_refp:
        return ""

    LAST_CONTROL_DATA = control_data

    state_airspeed = math.sqrt((state_velocity[0] - state_wind[0])**2 +
                               (state_velocity[1] - state_wind[1])**2 +
                               (state_velocity[2] - state_wind[2])**2)

    return (
        "t=%8d, " +
        "alt=%3.3f, alt_ref=%3.3f, " +
        "n=%6.2f, e=%6.2f, d=%6.2f, " +
        "tas=%5.2f, tas_ref=%5.2f, " +
        "yaw=%3.0f, yaw_ref=%3.0f, " +
        "pitch=%4.0f, pitch_ref=%4.0f, " +
        "roll=%4.0f, roll_ref=%4.0f, " +
        "t=%.3f, l=%.3f, r=%.3f, " +
        "objval=%10.1f, cycles=%9d, errors=%9d, resets=%9d, " +
        "path=%4d, last_gps=%4d, mode1=%d, nav_state=%d"
    ) % (
        (t, ) +
        (state_pos[2], control_refp.get("alt", 0)) +
        plog.lla_to_ned(
            (control_refp.get("lat", 0), control_refp.get("lon", 0),
                control_refp.get("alt", 0)),
            state_pos) +
        (state_airspeed, control_refp.get("airspeed", 0)) +
        (state_attitude[0], math.degrees(control_refp.get("yaw", 0))) +
        (state_attitude[1], math.degrees(control_refp.get("pitch", 0))) +
        (state_attitude[2], math.degrees(control_refp.get("roll", 0))) +
        (control_values[0], control_values[1], control_values[2]) +
        (control_obj_val, control_cycles, control_errors, control_resets) +
        (control_path, last_gps, control_mode[1], control_nav_version)
    )