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 variables
void 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
 */
void
GCS_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;
}