void MainWindow::on_button_switch_clicked()
{
    if(isStart){
        isStart = false;
        send_parameter(0, HEAD2_SWITCH_STOP);
    }
    else{
        isStart = true;
        send_parameter(0, HEAD2_SWITCH_START);
    }
}
//滑动条PID->C回调
void MainWindow::on_hSlider_pid_c_valueChanged(int value)
{
    int min = ui->lineEdit_min_c->text().toInt();
    int max = ui->lineEdit_max_c->text().toInt();
    float c = value/100.0f*(max-min)+min;
    QString c_str = QString("%1").arg(c);
    ui->lineEdit_pid_c->setText(c_str);
    send_parameter(c, HEAD2_C);
}
//滑动条PID->B回调
void MainWindow::on_hSlider_pid_b_valueChanged(int value)
{
    int min = ui->lineEdit_min_b->text().toInt();
    int max = ui->lineEdit_max_b->text().toInt();
    float b = value/100.0f*(max-min)+min;
    QString b_str = QString("%1").arg(b);
    ui->lineEdit_pid_b->setText(b_str);
    send_parameter(b, HEAD2_B);
}
//滑动条PID->A回调
void MainWindow::on_hSlider_pid_a_valueChanged(int value)
{
    int min = ui->lineEdit_min_a->text().toInt();
    int max = ui->lineEdit_max_a->text().toInt();
    float a = value/100.0f*(max-min)+min;
    QString a_str = QString("%1").arg(a);
    ui->lineEdit_pid_a->setText(a_str);
    send_parameter(a, HEAD2_A);
}
Esempio n. 5
0
// notify GCS of current value of parameter
void AP_Param::notify() const {
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    uint8_t idx;

    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);
    if (info == NULL) {
        // this is probably very bad
        return;
    }

    char name[AP_MAX_NAME_SIZE+1];
    copy_name_info(info, ginfo, idx, name, sizeof(name), true);

    uint32_t param_header_type;
    if (ginfo != NULL) {
        param_header_type = PGM_UINT8(&ginfo->type);
    } else {
        param_header_type = PGM_UINT8(&info->type);
    }

    send_parameter(name, (enum ap_var_type)param_header_type);
}
Esempio n. 6
0
// Save the variable to EEPROM, if supported
//
bool AP_Param::save(bool force_save)
{
    uint32_t group_element = 0;
    const struct GroupInfo *ginfo;
    uint8_t idx;
    const struct AP_Param::Info *info = find_var_info(&group_element, &ginfo, &idx);
    const AP_Param *ap;

    if (info == NULL) {
        // we don't have any info on how to store it
        return false;
    }

    struct Param_header phdr;

    // create the header we will use to store the variable
    if (ginfo != NULL) {
        phdr.type = PGM_UINT8(&ginfo->type);
    } else {
        phdr.type = PGM_UINT8(&info->type);
    }
    phdr.key  = PGM_UINT8(&info->key);
    phdr.group_element = group_element;

    ap = this;
    if (phdr.type != AP_PARAM_VECTOR3F && idx != 0) {
        // only vector3f can have non-zero idx for now
        return false;
    }
    if (idx != 0) {
        ap = (const AP_Param *)((uintptr_t)ap) - (idx*sizeof(float));
    }

    char name[AP_MAX_NAME_SIZE+1];
    copy_name_info(info, ginfo, idx, name, sizeof(name), true);

    // scan EEPROM to find the right location
    uint16_t ofs;
    if (scan(&phdr, &ofs)) {
        // found an existing copy of the variable
        eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
        send_parameter(name, (enum ap_var_type)phdr.type);
        return true;
    }
    if (ofs == (uint16_t) ~0) {
        return false;
    }

    // if the value is the default value then don't save
    if (phdr.type <= AP_PARAM_FLOAT) {
        float v1 = cast_to_float((enum ap_var_type)phdr.type);
        float v2;
        if (ginfo != NULL) {
            v2 = get_default_value(&ginfo->def_value);
        } else {
            v2 = get_default_value(&info->def_value);
        }
        if (is_equal(v1,v2) && !force_save) {
            GCS_MAVLINK::send_parameter_value_all(name, (enum ap_var_type)info->type, v2);
            return true;
        }
        if (phdr.type != AP_PARAM_INT32 &&
            (fabsf(v1-v2) < 0.0001f*fabsf(v1))) {
            // for other than 32 bit integers, we accept values within
            // 0.01 percent of the current value as being the same
            GCS_MAVLINK::send_parameter_value_all(name, (enum ap_var_type)info->type, v2);
            return true;
        }
    }

    if (ofs+type_size((enum ap_var_type)phdr.type)+2*sizeof(phdr) >= _storage.size()) {
        // we are out of room for saving variables
        hal.console->println("EEPROM full");
        return false;
    }

    // write a new sentinal, then the data, then the header
    write_sentinal(ofs + sizeof(phdr) + type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(ap, ofs+sizeof(phdr), type_size((enum ap_var_type)phdr.type));
    eeprom_write_check(&phdr, ofs, sizeof(phdr));

    send_parameter(name, (enum ap_var_type)phdr.type);
    return true;
}