#!/usr/bin/env python

"""Convert binary log generated by sdlog to CSV format
    
Usage: python logconv.py <log.bin>"""

__author__  = "Anton Babushkin"
__version__ = "0.1"

import struct, sys

def _unpack_packet(data):
    s = ""
    s += "Q"    #.timestamp = buf.raw.timestamp,
    s += "fff"  #.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
    s += "fff"  #.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
    s += "fff"  #.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
    s += "f"    #.baro = buf.raw.baro_pres_mbar,
    s += "f"    #.baro_alt = buf.raw.baro_alt_meter,
    s += "f"    #.baro_temp = buf.raw.baro_temp_celcius,
    s += "ffff" #.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
    s += "ffffffff"    #.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
    s += "f"    #.vbat = buf.batt.voltage_v,
    s += "f"    #.bat_current = buf.batt.current_a,
    s += "f"    #.bat_discharged = buf.batt.discharged_mah,
    s += "ffff" #.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
    s += "fff"  #.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
    s += "iii"  #.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
    s += "fff"  #.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
    s += "fffffffff"    #.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
    s += "fff"  #.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
    s += "ffff" #.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
    s += "ffffff"    #.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
    s += "f"    #.diff_pressure = buf.diff_pres.differential_pressure_pa,
    s += "f"    #.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
    s += "f"    #.true_airspeed = buf.airspeed.true_airspeed_m_s
    s += "iii"  # to align to 280
    d = struct.unpack(s, data)
    return d

def _main():
    if len(sys.argv) < 2:
        print "Usage:\npython logconv.py <log.bin>"
        return
    fn = sys.argv[1]
    sysvector_size = 280
    f = open(fn, "r")
    while True:
        data = f.read(sysvector_size)
        if len(data) < sysvector_size:
            break
        a = []
        for i in _unpack_packet(data):
            a.append(str(i))
        print ";".join(a)
    f.close()

if __name__ == "__main__":
    _main()