Subversion Repositories Projects

#include "Handler.h"
#include "FlightLog.h"
#include <sstream>
#include <string>

/**
 * Constructor that gets a communication instance
 */
Handler::Handler(Communication * com, KopterData * data) {
    this->com = com;
    this->data = data;
}

//-------------FlightCtrl commands--------------------
/**
 * read settings from FlightCtrl (settings index 0x00-0x05)
 */
void Handler::get_flightctrl_settings(int index) {
    FlightLog::info_FC("getting FlightCtrl settings");
    char tx_data[2] = {index, 0};
    com->send_cmd('q', ADDRESS_FC, tx_data, 1, true);
}

/**
 * write settings to FlightCtrl
 */
void Handler::set_flightctrl_settings(char * tx_data) {
    FlightLog::info_FC("setting FlightCtrl settings");
    com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
}

/**
 * test one or more motors
 */
void Handler::motor_test(sMotorData motor) {
    char tx_data[MAX_MOTORS];
    for (int z = 0; z<MAX_MOTORS; z++)
    {
        tx_data[z] = motor.desired_speed[z];
    }
    FlightLog::info_FC("testing motor");
    com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false);
}

void Handler::reset_motor() {
    for (int z = 0; z<MAX_MOTORS; z++)
    {
        data->motor.desired_speed[z] = 0;
    }

    motor_test(data->motor);
}

/**
 * read mixer values from FlightCtrl
 */
void Handler::read_motor_mixer() {
    FlightLog::info_FC("reading motor mixer values");
    char tx_data[1] = {0};
    //com->log("read motor mixer");
    com->send_cmd('n', ADDRESS_FC, tx_data, 1, true);
}

/**
 * write motor mixer values to FlightCtrl
 */
void Handler::write_motor_mixer(char * tx_data, int length) {
    FlightLog::info_FC("setting motor mixer values");
    com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
}

//FIXME: implement me!
int Handler::get_motor_config(char * tx_data) {
    FlightLog::error("get motor config not implemented");
    return -1;
}

//-------------NaviCtrl commands--------------------
/**
 * set debug values for NaviCtrl
 */
void Handler::set_navictrl_debug(int speed) {
    FlightLog::info_NC("setting NaviCtrl debug speed");
    char tx_data[1] = { speed };
    com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
}

/**
 * stop debug for NaviCtrl
 */
void Handler::stop_navictrl_debug() {
    set_navictrl_debug(0);
}

/**
 * send a waypoint to the NaviCtrl (the MikroKopter will fly to the position emidiately)
 */
void Handler::send_waypoint(Waypoint_t desired_pos) {
    FlightLog::info_GPS("sending Waypoint");
    com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false);
}

/**
 * add waypoint to waypoint list
 */
void Handler::add_waypoint(Waypoint_t wp) {
    FlightLog::info_GPS("adding Waypoint");
    com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
}

/**
 * clear waypoint list on MK
 */
void Handler::delete_waypoints() {
    Waypoint_t wp;
    wp.Position.Status = INVALID;
    send_waypoint(wp);
}
//-------------switch between Hardware--------------------
void Handler::switch_navictrl() {
    FlightLog::info("switching to NaviCtrl");
    char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
    com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
}

void Handler::switch_flightctrl() {
    FlightLog::info("switching to FlightCtrl");
    char tx_data[1] = { 0 };
    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}

void Handler::switch_mk3mag() {
    FlightLog::info("switching to MK3MAG");
    char tx_data[1] = { 1 };
    com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}

//-------------commands for MK3MAG-----------------


//-------------commands for all--------------------

/**
 * set debug values for all components
 */
void Handler::set_all_debug(int speed) {
    FlightLog::info("setting debug speed");
    char tx_data[1] = { speed };
    com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
}

/**
 * stop debug for all components
 */
void Handler::stop_all_debug() {
    set_all_debug(0);
}

/**
 * get all analog labels
 */
void Handler::get_analog() {
    FlightLog::info("get analog values");
    char tx_data[1] = { 0 };
    com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
}

/**
 * get values from LCD / show LCD
 */
void Handler::show_lcd() {
    FlightLog::info("show LCD");
    char tx_data[1] = {0};
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}

/**
 * got to next LCD Page
 */
void Handler::lcd_up() {
    FlightLog::info("going up one LCD page");
    char tx_data[2] = { 0, 0 };
    if (data->lcd_cur != data->lcd_max)
        tx_data[0] = data->lcd_cur+1;
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}

/**
 * got to previous LCD Page
 */
void Handler::lcd_down() {
    FlightLog::info("going down one LCD page");
    char tx_data[2] = { 0, 0 };
    if (data->lcd_cur != 0)
        tx_data[0] = data->lcd_cur-1;
    com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}

void Handler::get_version() {
    FlightLog::info("get version");
    //TODO: Check if is this correct or do we need data from switch_...
    char tx_data[1] = { 0 };
    com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
}

void Handler::get_ppm_channels() {
     FlightLog::info("get PPM channels");
    char tx_data[1] = { 0 };
    com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
}

void Handler::new_navi_data(char * data) {
    std::string mode;
    switch(data[N_NC_FLAGS])
    {
        case 0x01 : mode = "Free"; break;
        case 0x02 : mode = "Position Hold"; break;
        case 0x04 : mode = "Coming Home"; break;
        case 0x08 : mode = "Range Limit"; break;
        case 0x10 : mode = "Serial Error"; break;
        case 0x20 : mode = "Target reached"; break;
        case 0x40 : mode = "Manual Control"; break;
    }
   
}

/**
 * receive data
 */
void Handler::receive_data(char * incomming, int length) {
    if (incomming[0] != '#') {
        FlightLog::error("this frame is not correct");
        FlightLog::info(incomming);
        return;
    }
    int hardwareID = incomming[1] - 'a';

    //The cmd is also known as ID-Byte (or ID for short) in the wiki-dokumentation
    char cmd = incomming[2];
    //decoded data
    unsigned char data[MAX_DATA_SIZE];

    Parser::decode64(incomming, length, data, 3);

    switch(hardwareID)
    {
        case ADDRESS_FC :
            switch(cmd)
            {
                // Motor-Mixer
                case 'N' :
                    com->stop_resend();
                    //decoded data
                    FlightLog::info_FC("received motortest values from FlightCtrl");
                    if (data[0] == VERSION_MIXER)
                    {
                        //TODO: Handler::receivedMotorConfig(incomming)
                        //f_MotorMixer->set_MotorConfig(RX);
                    }
                    break;
                // Motor-Mixer Schreib-Bestätigung
                case 'M' :
                    com->stop_resend();

                    if (data[0] == 1)
                    {
                        FlightLog::info_FC("motor values written to FlightCtrl.");
                        //lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
                    } else {
                        FlightLog::warning("could not write motor values to FlightCtrl!");
                    }
                    break;

                // Stick-Belegung der Fernsteuerung
                case 'P' : // DONE 0.71g
                    FlightLog::info_FC("received stick-settings:");
                    for (int i = 1; i <= 8; i++) {
                        std::stringstream d;
                        d << "Stickvalue for Stick nr. " << i << ": " << Parser::dataToInt((char *)data, i*2, true);;
                        std::string val;
                        d >> val;
                        FlightLog::info_FC((char *)val.c_str());
                    }
                    /*f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode,  2,true));
                    f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode,  4,true));
                    f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode,  6,true));
                    f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode,  8,true));
                    f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true));
                    f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true));
                    f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true));
                    f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/
                    break;
                // Settings lesen
                case 'Q' : // DONE 0.71g
                    com->stop_resend();
                    FlightLog::info_FC("received settings");
                    if (data[1] == VERSION_SETTINGS)
                    {
                        int settings_id = data[0];
                        std::stringstream d;
                        d << "Settings-ID: " << settings_id;
                        std::string val;
                        d >> val;
                        FlightLog::info_FC((char *)val.c_str());
                        for (int i = 0; i < MaxParameter; i++) {
                            d.flush();
                            d << "Value for Parameter nr. " << i << ": " << (int)data[i+2];
                            d >> val;
                            FlightLog::info_FC((char *)val.c_str());
                        }
                       
                        /*for (int a = 0; a < MaxParameter; a++)
                        {
                            FCSettings[a] = RX.decode[a + 2];
                        }
                        f_Settings->show_FCSettings(Settings_ID, FCSettings);
                        f_Settings->pb_Read->setEnabled(true);
                        f_Settings->pb_Write->setEnabled(true);*/
                    }
                    else
                    {
                        FlightLog::error("wrong FlightCtrl version");
                        /*f_Settings->pb_Read->setDisabled(true);
                        f_Settings->pb_Write->setDisabled(true);

                        QString name = QString("Versionen inkompatibel.\n") +
                                    QString("Version von GroundStation benoetigt: ") +
                                    QString(VERSION_SETTINGS) +
                                    QString("\nVersion auf der FlightCtrl: ") +
                                    QString(RX.decode[1]) +
                                    QString("\nParameterbearbeitung nicht moeglich.");
                        QMessageBox::warning(this, QA_NAME,
                                name, QMessageBox::Ok);*/
                    }
                    break;
                // Settings written
                case 'S' : // DONE 0.71g
                    com->stop_resend();
                    FlightLog::info_FC("settings written successfully");
                    //TODO: QMessagebox("settings written successful") ?
                    break;
            }

        case ADDRESS_NC :
            switch(cmd)
            {
                // Navigationsdaten
                case 'O' : // NOT DONE 0.12h
                    //new_NaviData(RX);
                    FlightLog::info_NC("received navigation data");
                    break;
            }
//        case ADDRESS_MK3MAG :

        default :
            switch(cmd)
            {
                // LCD-Anzeige
                case 'L' : // DONE 0.71g
                    com->stop_resend();
                    FlightLog::info("received LCD page.");
                    /*int LCD[MAX_DATA_SIZE];
                    memcpy(LCD,RX.decode, sizeof(RX.decode));

                    f_LCD->show_Data(LCD);

                    LCD_Page     = RX.decode[0];
                    LCD_MAX_Page = RX.decode[1];
                    */
                    break;
                // Analoglabels
                case 'A' : // DONE 0.71g
                    com->stop_resend();
                    FlightLog::info("received analog labels");
                    //check position
                    if (data[0] != 31) {
                        /*
                        Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed();
                        if (Settings->Analog1.Label[Position] == "")
                        {
                            Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position);
                        }
                        Position ++;
                        TX_Data[0] = Position;
                        o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/
                    } else {
                        /*
                        for (int a = 0; a < MaxAnalog; a++)
                        {
                            lb_Analog[a]->setText(Settings->Analog1.Label[a]);
                        }
                        Settings->Analog1.Version = QString(Mode.Version);
                        Settings->write_Settings_AnalogLabels(HardwareID);
                        config_Plot();*/
                    }
                    break;
                // Debug-Daten
                case 'D' : // DONE 0.71g
                    FlightLog::info("received debug data:");
                    for (int i = 0; i < MaxAnalog; i++) {
                        /*
                        Parser::dataToInt((char *)data, (i * 2) + 2, false);
                        */
                    }
                    //show_DebugData();
                    break;
                // Version
                case 'V' : // DONE 0.71h
                    com->stop_resend();
                    FlightLog::info("received version");
                    /*
                    Mode.ID            = HardwareID;
                    Mode.VERSION_MAJOR = RX.decode[0];
                    Mode.VERSION_MINOR = RX.decode[1];
                    Mode.VERSION_PATCH = RX.decode[4];
                    Mode.VERSION_SERIAL_MAJOR = RX.decode[2];
                    Mode.VERSION_SERIAL_MINOR = RX.decode[3];

                    Mode.Hardware   = HardwareType[Mode.ID];
                    //TODO: Funktion im Handler get_version() oder sowas
                    QString version = QString("%1").arg(RX.decode[0]) + "." +
                                        QString("%1").arg(RX.decode[1]) +
                                        QString(RX.decode[4] + 'a');
                    Mode.Version = version.toLatin1().data;
                    setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " +
                                Mode.Hardware + " " +
                                Mode.Version);

                    if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR)
                    {
//                                AllowSend = false;
                            QMessageBox::warning(this, QA_NAME,
                            tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok);
                    }

                    if (ac_NoDebug->isChecked())
                    {
                        TX_Data[0] = 0;
                    }
                    else
                    if (ac_FastDebug->isChecked())
                    {
                        TX_Data[0] = Settings->Data.Debug_Fast / 10;
                    }
                    else
                    {
                        TX_Data[0] = Settings->Data.Debug_Slow / 10;
                    }

                    o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false);

                    // Wenn MK3MAG dann andauernd Daten neu anfragen.
                    if (Mode.ID == ADDRESS_MK3MAG)
                    {
                        TickerEvent[3] = true;
                        rb_SelMag->setChecked(true);
                    }

                    // Wenn NaviCtrl dann hier.
                    if (Mode.ID == ADDRESS_NC)
                    {
                        rb_SelNC->setChecked(true);

                        if (ac_NoNavi->isChecked())
                        {
                            TX_Data[0] = 0;
                        }
                        else
                        if (ac_FastNavi->isChecked())
                        {
                            TX_Data[0] = Settings->Data.Navi_Fast / 10;
                        }
                        else
                        {
                            TX_Data[0] = Settings->Data.Navi_Slow / 10;
                        }

                        o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false);
                    }


                    // Wenn FlightCtrl dann Settings abfragen.
                    if (Mode.ID == ADDRESS_FC)
                    {
                        rb_SelFC->setChecked(true);
                        {
                            TX_Data[0] = 0xff;
                            TX_Data[1] = 0;

                            // DEP: Raus wenn Resend implementiert.
//                                ToolBox::Wait(SLEEP);
                            o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true);
                            qDebug("FC - Get Settings");
                        }
                    }
                    // Wenn nicht Lesen und Schreiben der Settings deaktivieren.
                    else
                    {
                            f_Settings->pb_Read->setDisabled(true);
                            f_Settings->pb_Write->setDisabled(true);
                    }

                    Settings->read_Settings_Analog(HardwareID);
                    Settings->read_Settings_AnalogLabels(HardwareID);

                    if (Settings->Analog1.Version != QString(Mode.Version))
                    {
                        lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus."));
                        slot_ac_GetLabels();
                    }
                    else
                    for (int a = 0; a < MaxAnalog; a++)
                    {
                        lb_Analog[a]->setText(Settings->Analog1.Label[a]);
                    }
                    config_Plot();*/
                    break;
            }
    }
}