自学教程：C++ AP_Param类代码示例

void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg){    mavlink_param_request_read_t packet;    mavlink_msg_param_request_read_decode(msg, &packet);    enum ap_var_type p_type;    AP_Param *vp;    char param_name[AP_MAX_NAME_SIZE+1];    if (packet.param_index != -1) {        AP_Param::ParamToken token;        vp = AP_Param::find_by_index(packet.param_index, &p_type, &token);        if (vp == NULL) {            return;        }        vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true);        param_name[AP_MAX_NAME_SIZE] = 0;    } else {        strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE);        param_name[AP_MAX_NAME_SIZE] = 0;        vp = AP_Param::find(param_name, &p_type);        if (vp == NULL) {            return;        }    }        float value = vp->cast_to_float(p_type);    mavlink_msg_param_value_send_buf(        msg,        chan,        param_name,        value,        mav_var_type(p_type),        _count_parameters(),        packet.param_index);}

// print the value of all variablesvoid AP_Param::show_all(void){    ParamToken token;    AP_Param *ap;    enum ap_var_type type;    for (ap=AP_Param::first(&token, &type);         ap;         ap=AP_Param::next_scalar(&token, &type)) {        char s[AP_MAX_NAME_SIZE+1];        ap->copy_name_token(&token, s, sizeof(s), true);        s[AP_MAX_NAME_SIZE] = 0;        switch (type) {        case AP_PARAM_INT8:            hal.console->printf_P(PSTR("%s: %d/n"), s, (int)((AP_Int8 *)ap)->get());            break;        case AP_PARAM_INT16:            hal.console->printf_P(PSTR("%s: %d/n"), s, (int)((AP_Int16 *)ap)->get());            break;        case AP_PARAM_INT32:            hal.console->printf_P(PSTR("%s: %ld/n"), s, (long)((AP_Int32 *)ap)->get());            break;        case AP_PARAM_FLOAT:            hal.console->printf_P(PSTR("%s: %f/n"), s, ((AP_Float *)ap)->get());            break;        default:            break;        }    }}

FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :    Aircraft(home_str, frame_str){    use_time_sync = false;    rate_hz = 250 / target_speedup;    heli_demix = strstr(frame_str, "helidemix") != nullptr;    rev4_servos = strstr(frame_str, "rev4") != nullptr;    const char *colon = strchr(frame_str, ':');    if (colon) {        controller_ip = colon+1;    }    for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {        AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);        if (sim_defaults[i].save) {            enum ap_var_type ptype;            AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);            if (!p->configured()) {                p->save();            }        }    }    /* Create the thread that will be waiting for data from FlightAxis */    mutex = hal.util->new_semaphore();    int ret = pthread_create(&thread, NULL, update_thread, this);    if (ret != 0) {        AP_HAL::panic("SIM_FlightAxis: failed to create thread");    }}

/** * @brief Send the next pending parameter, called from deferred message * handling code */voidGCS_MAVLINK::queued_param_send(){    if (!initialised || _queued_parameter == NULL) {        return;    }    uint16_t bytes_allowed;    uint8_t count;    uint32_t tnow = hal.scheduler->millis();    // use at most 30% of bandwidth on parameters. The constant 26 is    // 1/(1000 * 1/8 * 0.001 * 0.3)    bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26;    if (bytes_allowed > comm_get_txspace(chan)) {        bytes_allowed = comm_get_txspace(chan);    }    count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);    // when we don't have flow control we really need to keep the    // param download very slow, or it tends to stall    if (!have_flow_control() && count > 5) {        count = 5;    }    while (_queued_parameter != NULL && count--) {        AP_Param      *vp;        float value;        // copy the current parameter and prepare to move to the next        vp = _queued_parameter;        // if the parameter can be cast to float, report it here and break out of the loop        value = vp->cast_to_float(_queued_parameter_type);        char param_name[AP_MAX_NAME_SIZE];        vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);        mavlink_msg_param_value_send(            chan,            param_name,            value,            mav_var_type(_queued_parameter_type),            _queued_parameter_count,            _queued_parameter_index);        _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);        _queued_parameter_index++;    }    _queued_parameter_send_time_ms = tnow;}

Morse::Morse(const char *home_str, const char *frame_str) :    Aircraft(home_str, frame_str){    char *saveptr = nullptr;    char *s = strdup(frame_str);    char *frame_option = strtok_r(s, ":", &saveptr);    char *args1 = strtok_r(nullptr, ":", &saveptr);    char *args2 = strtok_r(nullptr, ":", &saveptr);    char *args3 = strtok_r(nullptr, ":", &saveptr);    /*      allow setting of IP, sensors port and control port      format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT     */    if (args1) {        morse_ip = args1;    }    if (args2) {        morse_sensors_port = atoi(args2);        morse_control_port = morse_sensors_port+1;    }    if (args3) {        morse_control_port = atoi(args3);    }    if (strstr(frame_option, "-rover")) {        output_type = OUTPUT_ROVER;    } else if (strstr(frame_option, "-quad")) {        output_type = OUTPUT_QUAD;    } else if (strstr(frame_option, "-pwm")) {        output_type = OUTPUT_PWM;    } else {        // default to rover        output_type = OUTPUT_ROVER;    }    for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {        AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);        if (sim_defaults[i].save) {            enum ap_var_type ptype;            AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);            if (!p->configured()) {                p->save();            }        }    }    printf("Started Morse with %s:%u:%u type %u/n",           morse_ip, morse_sensors_port, morse_control_port,           (unsigned)output_type);}

/*  find the def_value for a variable by name*/const float *AP_Param::find_def_value_ptr(const char *name){    enum ap_var_type ptype;    AP_Param *vp = find(name, &ptype);    if (vp == NULL) {        return NULL;    }    uint32_t group_element;    const struct GroupInfo *ginfo;    uint8_t gidx;    const struct AP_Param::Info *info = vp->find_var_info(&group_element, &ginfo, &gidx);    if (info == NULL) {        return NULL;    }    if (ginfo != NULL) {        return &ginfo->def_value;    }    return &info->def_value;}

void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash){    mavlink_param_set_t packet;    mavlink_msg_param_set_decode(msg, &packet);    enum ap_var_type var_type;    // set parameter    AP_Param *vp;    char key[AP_MAX_NAME_SIZE+1];    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);    key[AP_MAX_NAME_SIZE] = 0;    // find existing param so we can get the old value    vp = AP_Param::find(key, &var_type);    if (vp == NULL) {        return;    }    float old_value = vp->cast_to_float(var_type);    // set the value    vp->set_float(packet.param_value, var_type);    /*      we force the save if the value is not equal to the old      value. This copes with the use of override values in      constructors, such as PID elements. Otherwise a set to the      default value which differs from the constructor value doesn't      save the change     */    bool force_save = !is_equal(packet.param_value, old_value);    // save the change    vp->save(force_save);    if (DataFlash != NULL) {        DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));    }}

void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash){    mavlink_param_set_t packet;    mavlink_msg_param_set_decode(msg, &packet);    enum ap_var_type var_type;    // set parameter    AP_Param *vp;    char key[AP_MAX_NAME_SIZE+1];    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);    key[AP_MAX_NAME_SIZE] = 0;    vp = AP_Param::set_param_by_name(key, packet.param_value, &var_type);    if (vp == NULL) {        return;    }    // save the change    vp->save();    // Report back the new value if we accepted the change    // we send the value we actually set, which could be    // different from the value sent, in case someone sent    // a fractional value to an integer type    mavlink_msg_param_value_send_buf(        msg,        chan,        key,        vp->cast_to_float(var_type),        mav_var_type(var_type),        _count_parameters(),        -1);     // XXX we don't actually know what its index is...    if (DataFlash != NULL) {        DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));    }}

void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash){    mavlink_param_set_t packet;    mavlink_msg_param_set_decode(msg, &packet);    enum ap_var_type var_type;    // set parameter    AP_Param *vp;    char key[AP_MAX_NAME_SIZE+1];    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);    key[AP_MAX_NAME_SIZE] = 0;    // find existing param so we can get the old value    vp = AP_Param::find(key, &var_type);    if (vp == NULL) {        return;    }    float old_value = vp->cast_to_float(var_type);    // set the value    vp->set_float(packet.param_value, var_type);    /*      we force the save if the value is not equal to the old      value. This copes with the use of override values in      constructors, such as PID elements. Otherwise a set to the      default value which differs from the constructor value doesn't      save the change     */    bool force_save = !is_equal(packet.param_value, old_value);    // save the change    vp->save(force_save);    // Report back the new value if we accepted the change    // we send the value we actually set, which could be    // different from the value sent, in case someone sent    // a fractional value to an integer type    mavlink_msg_param_value_send_buf(        msg,        chan,        key,        vp->cast_to_float(var_type),        mav_var_type(var_type),        _count_parameters(),        -1);     // XXX we don't actually know what its index is...    if (DataFlash != NULL) {        DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));    }}

void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash){    AP_Param *vp;    enum ap_var_type var_type;    mavlink_param_set_t packet;    mavlink_msg_param_set_decode(msg, &packet);    if (mavlink_check_target(packet.target_system, packet.target_component)) {        return;    }    // set parameter    char key[AP_MAX_NAME_SIZE+1];    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);    key[AP_MAX_NAME_SIZE] = 0;    // find the requested parameter    vp = AP_Param::find(key, &var_type);    if ((NULL != vp) &&                                 // exists        !isnan(packet.param_value) &&                       // not nan        !isinf(packet.param_value)) {                       // not inf        // add a small amount before casting parameter values        // from float to integer to avoid truncating to the        // next lower integer value.        float rounding_addition = 0.01;                // handle variables with standard type IDs        if (var_type == AP_PARAM_FLOAT) {            ((AP_Float *)vp)->set_and_save(packet.param_value);        } else if (var_type == AP_PARAM_INT32) {            if (packet.param_value < 0) rounding_addition = -rounding_addition;            float v = packet.param_value+rounding_addition;            v = constrain_float(v, -2147483648.0, 2147483647.0);            ((AP_Int32 *)vp)->set_and_save(v);        } else if (var_type == AP_PARAM_INT16) {            if (packet.param_value < 0) rounding_addition = -rounding_addition;            float v = packet.param_value+rounding_addition;            v = constrain_float(v, -32768, 32767);            ((AP_Int16 *)vp)->set_and_save(v);        } else if (var_type == AP_PARAM_INT8) {            if (packet.param_value < 0) rounding_addition = -rounding_addition;            float v = packet.param_value+rounding_addition;            v = constrain_float(v, -128, 127);            ((AP_Int8 *)vp)->set_and_save(v);        } else {            // we don't support mavlink set on this parameter            return;        }        // Report back the new value if we accepted the change        // we send the value we actually set, which could be        // different from the value sent, in case someone sent        // a fractional value to an integer type        mavlink_msg_param_value_send_buf(            msg,            chan,            key,            vp->cast_to_float(var_type),            mav_var_type(var_type),            _count_parameters(),            -1);     // XXX we don't actually know what its index is...        if (DataFlash != NULL) {            DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));        }    }}

/*  load a default set of parameters from a file */bool AP_Param::load_defaults_file(const char *filename){    FILE *f = fopen(filename, "r");    if (f == NULL) {        return false;    }    char line[100];    /*      work out how many parameter default structures to allocate     */    uint16_t num_defaults = 0;    while (fgets(line, sizeof(line)-1, f)) {        char *pname;        float value;        if (!parse_param_line(line, &pname, value)) {            continue;        }        if (!find_def_value_ptr(pname)) {            fclose(f);            return false;        }        num_defaults++;    }    fclose(f);    if (param_overrides != NULL) {        free(param_overrides);    }    num_param_overrides = 0;    param_overrides = new param_override[num_defaults];    if (param_overrides == NULL) {        return false;    }    /*        re-open to avoid possible seek issues with NuttX     */    f = fopen(filename, "r");    if (f == NULL) {        return false;    }    uint16_t idx = 0;    while (fgets(line, sizeof(line)-1, f)) {        char *pname;        float value;        if (!parse_param_line(line, &pname, value)) {            continue;        }        const float *def_value_ptr = find_def_value_ptr(pname);        if (!def_value_ptr) {            fclose(f);            return false;        }        param_overrides[idx].def_value_ptr = def_value_ptr;        param_overrides[idx].value = value;        idx++;        enum ap_var_type var_type;        AP_Param *vp = AP_Param::find(pname, &var_type);        if (!vp) {            fclose(f);            return false;        }        vp->set_float(value, var_type);    }    fclose(f);    num_param_overrides = num_defaults;    return true;}