自学教程：C++ uint16_encode函数代码示例

/**@brief Function for encoding a Glucose measurement. * * @param[in]  p_meas            Measurement to be encoded. * @param[out] p_encoded_buffer  Pointer to buffer where the encoded measurement is to be stored. * * @return Size of encoded measurement. */static uint8_t gls_meas_encode(const ble_gls_meas_t * p_meas, uint8_t * p_encoded_buffer){    uint8_t len = 0;    p_encoded_buffer[len++] = p_meas->flags;    len += uint16_encode(p_meas->sequence_number, &p_encoded_buffer[len]);    len += ble_date_time_encode(&p_meas->base_time, &p_encoded_buffer[len]);    if (p_meas->flags & BLE_GLS_MEAS_FLAG_TIME_OFFSET)    {        len += uint16_encode(p_meas->time_offset, &p_encoded_buffer[len]);    }    if (p_meas->flags & BLE_GLS_MEAS_FLAG_CONC_TYPE_LOC)    {        uint16_t encoded_concentration;        encoded_concentration = ((p_meas->glucose_concentration.exponent << 12) & 0xF000) |                                ((p_meas->glucose_concentration.mantissa <<  0) & 0x0FFF);        p_encoded_buffer[len++] = (uint8_t)(encoded_concentration);        p_encoded_buffer[len++] = (uint8_t)(encoded_concentration >> 8);        p_encoded_buffer[len++] = (p_meas->sample_location << 4) | (p_meas->type & 0x0F);    }

uint32_t ble_gattc_primary_services_discover_req_enc(uint16_t                 conn_handle,                                                     uint16_t                 start_handle,                                                     ble_uuid_t const * const p_srvc_uuid,                                                     uint8_t * const          p_buf,                                                     uint32_t *               p_buf_len){    uint32_t index = 0;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    SER_ASSERT_LENGTH_LEQ(index + 5, *p_buf_len);    p_buf[index++] = SD_BLE_GATTC_PRIMARY_SERVICES_DISCOVER;    index         += uint16_encode(conn_handle, &p_buf[index]);    index         += uint16_encode(start_handle, &p_buf[index]);    SER_ASSERT_LENGTH_LEQ(index + 1, *p_buf_len);    p_buf[index++] = (p_srvc_uuid != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    if (p_srvc_uuid != NULL)    {        SER_ASSERT_LENGTH_LEQ(index + 3, *p_buf_len);        index         += uint16_encode(p_srvc_uuid->uuid, &p_buf[index]);        p_buf[index++] = p_srvc_uuid->type;    }    *p_buf_len = index;    return NRF_SUCCESS;}

uint32_t count16_cond_data16_enc(uint16_t const * const p_data,                                 uint16_t const         count,                                 uint8_t * const        p_buf,                                 uint32_t               buf_len,                                 uint32_t * const       p_index){    uint32_t i = 0;    SER_ASSERT_LENGTH_LEQ(3, ((int32_t)buf_len - *p_index));    *p_index += uint16_encode(count, &p_buf[*p_index]);    if (p_data)    {        SER_ASSERT_LENGTH_LEQ((int32_t)(2 * count + 1), ((int32_t)buf_len - *p_index));        p_buf[*p_index] = SER_FIELD_PRESENT;        *p_index       += 1;        //memcpy may fail in case of Endianness difference between application and connectivity processor        for (i = 0; i < count; i++)        {            *p_index += uint16_encode(p_data[i], &p_buf[*p_index]);        }    }    else    {        SER_ASSERT_LENGTH_LEQ((1), ((int32_t)buf_len - *p_index));        p_buf[*p_index] = SER_FIELD_NOT_PRESENT;        *p_index       += 1;    }    return NRF_SUCCESS;}

uint32_t ble_rpc_evt_gatts_encode(ble_evt_t * p_ble_evt, uint8_t * p_buffer){    uint32_t index    = 0;    // Encode packet type.    p_buffer[index++] = BLE_RPC_PKT_EVT;    // Encode header.    index += uint16_encode(p_ble_evt->header.evt_id,  &p_buffer[index]);    // Encode common part of the event.    index += uint16_encode(p_ble_evt->evt.gatts_evt.conn_handle, &p_buffer[index]);    // Encode events.    switch (p_ble_evt->header.evt_id)    {        case BLE_GATTS_EVT_WRITE:            index += gatts_write_evt_encode(p_ble_evt, &p_buffer[index]);            break;        case BLE_GATTS_EVT_SYS_ATTR_MISSING:            index += gatts_sys_attr_missing_encode(p_ble_evt, &p_buffer[index]);            break;        default:            // No implementation needed.            break;    }    return index;}

uint32_t ble_version_get_rsp_enc(uint32_t                    return_code,                                 uint8_t * const             p_buf,                                 uint32_t * const            p_buf_len,                                 ble_version_t const * const p_version){    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    uint32_t total_len = *p_buf_len;    uint32_t err_code = ser_ble_cmd_rsp_status_code_enc(SD_BLE_VERSION_GET, return_code,                                                        p_buf, p_buf_len);    if (err_code != NRF_SUCCESS)    {        return err_code;    }    if (return_code != NRF_SUCCESS)    {        return NRF_SUCCESS;    }    SER_ASSERT_NOT_NULL(p_version);    uint32_t index = *p_buf_len;    SER_ASSERT_LENGTH_LEQ(index + 5, total_len);    p_buf[index++] = p_version->version_number;    index         += uint16_encode(p_version->company_id, &p_buf[index]);    index         += uint16_encode(p_version->subversion_number, &p_buf[index]);    *p_buf_len = index;    return NRF_SUCCESS;}

static uint32_t conn_int_encode(const ble_advdata_conn_int_t * p_conn_int,                                uint8_t *                      p_encoded_data,                                uint8_t *                      p_len){    uint32_t err_code;    // Check for buffer overflow    if ((*p_len) + 2 + 2 * sizeof(uint16_le_t) > BLE_GAP_ADV_MAX_SIZE)    {        return NRF_ERROR_DATA_SIZE;    }        // Check parameters    err_code = conn_int_check(p_conn_int);    if (err_code != NRF_SUCCESS)    {        return err_code;    }        // Encode Length and AD Type    p_encoded_data[(*p_len)++] = 1 + 2 * sizeof(uint16_le_t);    p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_SLAVE_CONNECTION_INTERVAL_RANGE;        // Encode Minimum and Maximum Connection Intervals    (*p_len) += uint16_encode(p_conn_int->min_conn_interval, &p_encoded_data[*p_len]);    (*p_len) += uint16_encode(p_conn_int->max_conn_interval, &p_encoded_data[*p_len]);        return NRF_SUCCESS;}

uint32_t sd_ble_gap_ppcp_set(ble_gap_conn_params_t const * const p_conn_params){    uint32_t index = 0;    g_cmd_buffer[index++] = BLE_RPC_PKT_CMD;    g_cmd_buffer[index++] = SD_BLE_GAP_PPCP_SET;    if (p_conn_params != NULL)    {        g_cmd_buffer[index++] = RPC_BLE_FIELD_PRESENT;        index += uint16_encode(p_conn_params->min_conn_interval, &g_cmd_buffer[index]);        index += uint16_encode(p_conn_params->max_conn_interval, &g_cmd_buffer[index]);        index += uint16_encode(p_conn_params->slave_latency    , &g_cmd_buffer[index]);        index += uint16_encode(p_conn_params->conn_sup_timeout, &g_cmd_buffer[index]);    }    else    {        g_cmd_buffer[index++] = RPC_BLE_FIELD_NOT_PRESENT;    }    uint32_t err_code = hci_transport_pkt_write(g_cmd_buffer, index);    if (err_code != NRF_SUCCESS)    {        return err_code;    }    err_code = ble_rpc_cmd_resp_wait(SD_BLE_GAP_PPCP_SET);    UNUSED_VARIABLE(hci_transport_rx_pkt_consume(g_cmd_response_buf));    return err_code;}

/**@brief Function for encoding a CSCS Measurement. * * @param[in]   p_cscs              Cycling Speed and Cadence Service structure. * @param[in]   p_csc_measurement   Measurement to be encoded. * @param[out]  p_encoded_buffer    Buffer where the encoded data will be written. * * @return      Size of encoded data. */static uint8_t csc_measurement_encode(ble_cscs_t *       p_cscs,                                      ble_cscs_meas_t *  p_csc_measurement,                                      uint8_t *          p_encoded_buffer){    uint8_t flags = 0;    uint8_t len   = 1;    // Cumulative Wheel Revolutions and Last Wheel Event Time Fields    if (p_cscs->feature & BLE_CSCS_FEATURE_WHEEL_REV_BIT)    {        if (p_csc_measurement->is_wheel_rev_data_present)        {            flags |= CSC_MEAS_FLAG_MASK_WHEEL_REV_DATA_PRESENT;            len += uint32_encode(p_csc_measurement->cumulative_wheel_revs, &p_encoded_buffer[len]);            len += uint16_encode(p_csc_measurement->last_wheel_event_time, &p_encoded_buffer[len]);        }    }        // Cumulative Crank Revolutions and Last Crank Event Time Fields    if (p_cscs->feature & BLE_CSCS_FEATURE_CRANK_REV_BIT)    {        if (p_csc_measurement->is_crank_rev_data_present)        {            flags |= CSC_MEAS_FLAG_MASK_CRANK_REV_DATA_PRESENT;            len += uint16_encode(p_csc_measurement->cumulative_crank_revs, &p_encoded_buffer[len]);            len += uint16_encode(p_csc_measurement->last_crank_event_time, &p_encoded_buffer[len]);        }    }        // Flags Field    p_encoded_buffer[0] = flags;        return len;}

uint32_t ble_gap_authenticate_req_enc(uint16_t                           conn_handle,                                      ble_gap_sec_params_t const * const p_sec_params,                                      uint8_t * const                    p_buf,                                      uint32_t * const                   p_buf_len){    uint32_t index = 0;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    SER_ASSERT_LENGTH_LEQ(1 + 2 + 1, *p_buf_len);    p_buf[index++] = SD_BLE_GAP_AUTHENTICATE;    index         += uint16_encode(conn_handle, &p_buf[index]);    p_buf[index++] = (p_sec_params != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    if (p_sec_params != NULL)    {        SER_ASSERT_LENGTH_LEQ(index + 2 + 1 + 1 + 1, *p_buf_len);        index         += uint16_encode(p_sec_params->timeout, &p_buf[index]);        p_buf[index++] = ((p_sec_params->oob << 5) |                          (p_sec_params->io_caps << 2) |                          (p_sec_params->mitm << 1) |                          (p_sec_params->bond << 0));        p_buf[index++] = p_sec_params->min_key_size;        p_buf[index++] = p_sec_params->max_key_size;    }    *p_buf_len = index;    return NRF_SUCCESS;}

static uint32_t conn_int_encode(const ble_advdata_conn_int_t * p_conn_int,                                uint8_t                      * p_encoded_data,                                uint16_t                     * p_offset,                                uint16_t                       max_size){    uint32_t err_code;    // Check for buffer overflow.    if (((*p_offset) + AD_TYPE_CONN_INT_SIZE) > max_size)    {        return NRF_ERROR_DATA_SIZE;    }    // Check parameters.    err_code = conn_int_check(p_conn_int);    if (err_code != NRF_SUCCESS)    {        return err_code;    }    // Encode Length and AD Type.    p_encoded_data[*p_offset]  = (uint8_t)(ADV_AD_TYPE_FIELD_SIZE + AD_TYPE_CONN_INT_DATA_SIZE);    *p_offset                 += ADV_LENGTH_FIELD_SIZE;    p_encoded_data[*p_offset]  = BLE_GAP_AD_TYPE_SLAVE_CONNECTION_INTERVAL_RANGE;    *p_offset                 += ADV_AD_TYPE_FIELD_SIZE;    // Encode Minimum and Maximum Connection Intervals.    *p_offset += uint16_encode(p_conn_int->min_conn_interval, &p_encoded_data[*p_offset]);    *p_offset += uint16_encode(p_conn_int->max_conn_interval, &p_encoded_data[*p_offset]);    return NRF_SUCCESS;}

static uint8_t encode_adv_packet(uint8_t * p_data){    uint8_t offset    = 0;    // Adding advertising data: Flags    p_data[offset++] =  ADV_TYPE_LEN;    p_data[offset++] =  BLE_GAP_AD_TYPE_FLAGS;    p_data[offset++] =  BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;    // Adding advertising data: Manufacturer specific data.    p_data[offset++] =  ADV_DATA_MANUF_DATA_LEN + 1;                              p_data[offset++] =  BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA;    offset           += uint16_encode(m_beacon.manuf_id, &p_data[offset]);    p_data[offset++] =  APP_DEVICE_TYPE;    // Adding manufacturer specific data (beacon data).    p_data[offset++] = ADV_DATA_BEACON_DATA_LEN;     memcpy(&p_data[offset], &m_beacon.uuid, sizeof(ble_uuid128_t));    offset += sizeof(ble_uuid128_t);    offset += uint16_encode(m_beacon.major, &p_data[offset]);    offset += uint16_encode(m_beacon.minor, &p_data[offset]);    p_data[offset++] = m_beacon.rssi;       return offset;}

uint32_t ble_gattc_characteristics_discover_req_enc(    uint16_t                               conn_handle,    ble_gattc_handle_range_t const * const p_handle_range,    uint8_t * const                        p_buf,    uint32_t *                             p_buf_len){    uint32_t index = 0;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    SER_ASSERT_LENGTH_LEQ(index + 1 + 2, *p_buf_len);    p_buf[index++] = SD_BLE_GATTC_CHARACTERISTICS_DISCOVER;    index         += uint16_encode(conn_handle, &p_buf[index]);    SER_ASSERT_LENGTH_LEQ(index + 1, *p_buf_len);    p_buf[index++] = (p_handle_range != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    if (p_handle_range != NULL)    {        SER_ASSERT_LENGTH_LEQ(index + 2 + 2, *p_buf_len);        index += uint16_encode(p_handle_range->start_handle, &p_buf[index]);        index += uint16_encode(p_handle_range->end_handle, &p_buf[index]);    }    *p_buf_len = index;    return NRF_SUCCESS;}

uint32_t ble_gatts_hvx_req_enc(uint16_t                             conn_handle,                               ble_gatts_hvx_params_t const * const p_hvx_params,                               uint8_t * const                      p_buf,                               uint32_t * const                     p_buf_len){    uint32_t index = 0;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    SER_ASSERT(p_hvx_params == NULL ||               !(p_hvx_params->p_len == NULL && p_hvx_params->p_data != NULL), NRF_ERROR_NULL);    SER_ASSERT_LENGTH_LEQ(index + 1 + 2 + 1 + 1, *p_buf_len);    p_buf[index++] = SD_BLE_GATTS_HVX;    index         += uint16_encode(conn_handle, &p_buf[index]);    p_buf[index++] = (p_hvx_params != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    if (p_hvx_params != NULL)    {        SER_ASSERT_LENGTH_LEQ(index + 2 + 1 + 2 + 2, *p_buf_len);        index         += uint16_encode(p_hvx_params->handle, &p_buf[index]);        p_buf[index++] = p_hvx_params->type;        index         += uint16_encode(p_hvx_params->offset, &p_buf[index]);        if (p_hvx_params->p_len != NULL)        {            SER_ASSERT_LENGTH_LEQ(index + 1 + 2 + 1, *p_buf_len);            SER_ERROR_CHECK(*p_hvx_params->p_len <= BLE_GATTS_VAR_ATTR_LEN_MAX,                            NRF_ERROR_INVALID_PARAM);            p_buf[index++] = SER_FIELD_PRESENT;            index         += uint16_encode(*(p_hvx_params->p_len), &p_buf[index]);            if (p_hvx_params->p_data != NULL)            {                SER_ASSERT_LENGTH_LEQ(index + 1 + *(p_hvx_params->p_len), *p_buf_len);                p_buf[index++] = SER_FIELD_PRESENT;                memcpy(&(p_buf[index]), p_hvx_params->p_data, *(p_hvx_params->p_len));                index += *(p_hvx_params->p_len);            }            else            {                p_buf[index++] = SER_FIELD_NOT_PRESENT;            }        }        else        {            p_buf[index++] = SER_FIELD_NOT_PRESENT;            p_buf[index++] = SER_FIELD_NOT_PRESENT;        }    }    *p_buf_len = index;    return NRF_SUCCESS;}

/**@brief Function for encoding a ITU Sensor Service Measurement. * * @param[in]   p_iss			         ITU Sensor Service structure. * @param[in]   measurement        Measurement to be encoded. * @param[out]  p_encoded_buffer   Buffer where the encoded data will be written. * * @return      Size of encoded data. */static uint8_t iss_measurement_encode(iss_t * p_iss, int32_t * measurement,uint8_t * p_encoded_buffer,uint32_t * value){    uint8_t flags = 0;    uint8_t len   = 1;			//Value always present		*value = (((p_iss->IEEE_exponent << 24) & 0xFF000000) | (*measurement & 0x00FFFFFF));		len += uint32_encode(*value, &p_encoded_buffer[len]);				//Remember, to increament the seq nr so we have a continuous flow		++(p_iss->curr_seq_nr);			// Space + Time value    if (p_iss->space_time_present)    {        flags |= ITS_MEAS_FLAG_SPACE_TIME_BIT;				p_encoded_buffer[len++]  = p_iss->coord;        len   += uint16_encode(p_iss->curr_seq_nr, &p_encoded_buffer[len]);    }	    // Unit value    if (p_iss->unit_present)    {        flags |= ITS_MEAS_FLAG_UNIT_BIT;        len   += uint16_encode(p_iss->unit, &p_encoded_buffer[len]);		}				// Type value    if (p_iss->type_make_present)    {        flags |= ITS_MEAS_FLAG_TYPE_MAKE_BIT;        p_encoded_buffer[len++]  = p_iss->type;				p_encoded_buffer[len++]  = p_iss->make;		}				// ID field    if (p_iss->ID_present)    {        flags |= ITS_MEAS_FLAG_ID_BIT;        len   += uint16_encode(p_iss->ID, &p_encoded_buffer[len]);    }				// Sampling Frequency field    if (p_iss->samp_freq_present)    {        flags |= ITS_MEAS_FLAG_SAMP_FREQ_BIT;        len   += uint32_encode(p_iss->samp_freq_in_m_sec, &p_encoded_buffer[len]);    }    // Flags field    p_encoded_buffer[0] = flags;    return len;}

/**@brief Function for encoding a ITU Sensor Service Configuration. * * @param[in]   p_iss			         ITU Sensor Service structure. * @param[out]  p_encoded_buffer   Buffer where the encoded data will be written. * * @return      Size of encoded data. */static uint8_t its_conf_encode(iss_t * p_iss, uint8_t * p_encoded_buffer){		uint8_t pres_flags = 0;    uint8_t len   = 1;			/*    // Sequence number field    if (p_iss->space_time_present)    {        pres_flags |= ITS_MEAS_FLAG_SPACE_TIME_BIT;    }			// Unit value    if (p_iss->unit_present)    {        pres_flags |= ITS_MEAS_FLAG_UNIT_BIT;		}				// Type value    if (p_iss->type_make_present)    {        pres_flags |= ITS_MEAS_FLAG_TYPE_MAKE_BIT;		}				// ID field    if (p_iss->ID_present)    {        pres_flags |= ITS_MEAS_FLAG_ID_BIT;    }				// Sampling Frequency field    if (p_iss->samp_freq_present)    {        pres_flags |= ITS_MEAS_FLAG_SAMP_FREQ_BIT;    }*/				pres_flags |= ITS_MEAS_FLAG_SPACE_TIME_BIT;		pres_flags |= ITS_MEAS_FLAG_UNIT_BIT;		pres_flags |= ITS_MEAS_FLAG_TYPE_MAKE_BIT;		pres_flags |= ITS_MEAS_FLAG_ID_BIT;		pres_flags |= ITS_MEAS_FLAG_SAMP_FREQ_BIT;    // Presentation Flag field    p_encoded_buffer[0] = pres_flags;				p_encoded_buffer[len++]  = p_iss->coord;		len += uint16_encode(p_iss->curr_seq_nr, &p_encoded_buffer[len]);		len += uint16_encode(p_iss->unit, &p_encoded_buffer[len]);		p_encoded_buffer[len++]  = p_iss->type;		p_encoded_buffer[len++]  = p_iss->make;		len += uint16_encode(p_iss->ID, &p_encoded_buffer[len]);		len += uint32_encode(p_iss->samp_freq_in_m_sec, &p_encoded_buffer[len]);    return len;}

/**@brief Function for encoding a Glucose measurement. * * @param[in]  p_meas           Measurement to be encoded. * @param[out] p_encoded_buffer Pointer to buffer where the encoded measurement is to be stored. * * @return Size of encoded measurement. */static uint8_t cgms_meas_encode(nrf_ble_cgms_t            * p_cgms,                                const nrf_ble_cgms_meas_t * p_meas,                                uint8_t                   * p_encoded_buffer){    uint8_t len = 2;    uint8_t flags = p_meas->flags;    len += uint16_encode(p_meas->glucose_concentration,                         &p_encoded_buffer[len]);    len += uint16_encode(p_meas->time_offset,                         &p_encoded_buffer[len]);    if (p_meas->sensor_status_annunciation.warning != 0)    {        p_encoded_buffer[len++] = p_meas->sensor_status_annunciation.warning;        flags                  |= NRF_BLE_CGMS_STATUS_FLAGS_WARNING_OCT_PRESENT;    }    if (p_meas->sensor_status_annunciation.calib_temp != 0)    {        p_encoded_buffer[len++] = p_meas->sensor_status_annunciation.calib_temp;        flags                  |= NRF_BLE_CGMS_STATUS_FLAGS_CALTEMP_OCT_PRESENT;    }    if (p_meas->sensor_status_annunciation.status != 0)    {        p_encoded_buffer[len++] = p_meas->sensor_status_annunciation.status;        flags                  |= NRF_BLE_CGMS_STATUS_FLAGS_STATUS_OCT_PRESENT;    }    // Trend field    if (p_cgms->feature.feature & NRF_BLE_CGMS_FEAT_CGM_TREND_INFORMATION_SUPPORTED)    {        if (flags & NRF_BLE_CGMS_FLAG_TREND_INFO_PRESENT)        {            len += uint16_encode(p_meas->trend, &p_encoded_buffer[len]);        }    }    // Quality field    if (p_cgms->feature.feature & NRF_BLE_CGMS_FEAT_CGM_QUALITY_SUPPORTED)    {        if (flags & NRF_BLE_CGMS_FLAGS_QUALITY_PRESENT)        {            len += uint16_encode(p_meas->quality, &p_encoded_buffer[len]);        }    }    p_encoded_buffer[1] = flags;    p_encoded_buffer[0] = len;    return len;}

/**@brief Function for encoding a Heart Rate Measurement. * * @param[in]   p_hrs              Heart Rate Service structure. * @param[in]   heart_rate         Measurement to be encoded. * @param[out]  p_encoded_buffer   Buffer where the encoded data will be written. * * @return      Size of encoded data. */static uint8_t hrm_encode(ble_hrs_t * p_hrs, uint16_t heart_rate, uint8_t * p_encoded_buffer){    uint8_t flags = 0;    uint8_t len   = 1;    int     i;    // Set sensor contact related flags    if (p_hrs->is_sensor_contact_supported)    {        flags |= HRM_FLAG_MASK_SENSOR_CONTACT_SUPPORTED;    }    if (p_hrs->is_sensor_contact_detected)    {        flags |= HRM_FLAG_MASK_SENSOR_CONTACT_DETECTED;    }    // Encode heart rate measurement    if (heart_rate > 0xff)    {        flags |= HRM_FLAG_MASK_HR_VALUE_16BIT;        len   += uint16_encode(heart_rate, &p_encoded_buffer[len]);    }    else    {        p_encoded_buffer[len++] = (uint8_t)heart_rate;    }    // Encode rr_interval values    if (p_hrs->rr_interval_count > 0)    {        flags |= HRM_FLAG_MASK_RR_INTERVAL_INCLUDED;    }    for (i = 0; i < p_hrs->rr_interval_count; i++)    {        if (len + sizeof(uint16_t) > MAX_HRM_LEN)        {            // Not all stored rr_interval values can fit into the encoded hrm,            // move the remaining values to the start of the buffer.            memmove(&p_hrs->rr_interval[0],                    &p_hrs->rr_interval[i],                    (p_hrs->rr_interval_count - i) * sizeof(uint16_t));            break;        }        len += uint16_encode(p_hrs->rr_interval[i], &p_encoded_buffer[len]);    }    p_hrs->rr_interval_count -= i;    // Add flags    p_encoded_buffer[0] = flags;    return len;}

void ant_bpwr_page_torque_encode(uint8_t                           * p_page_buffer,                                 ant_bpwr_page_torque_data_t const * p_page_data){    ant_bpwr_page_torque_data_layout_t * p_outcoming_data =        (ant_bpwr_page_torque_data_layout_t *)p_page_buffer;    p_outcoming_data->update_event_count    = p_page_data->update_event_count;    p_outcoming_data->tick                  = p_page_data->tick;    UNUSED_PARAMETER(uint16_encode(p_page_data->period, p_outcoming_data->period));    UNUSED_PARAMETER(uint16_encode(p_page_data->accumulated_torque,                                   p_outcoming_data->accumulated_torque));}

/** @brief Function for encoding the BLE_GATTS_EVT_WRITE event. * * @param[in]   p_ble_evt       Input BLE event. * @param[out]  p_buffer        Pointer to a buffer for the encoded event. * * @return Number of bytes encoded. */static uint32_t gatts_write_evt_encode(const ble_evt_t * const p_ble_evt,                                       uint8_t * const         p_buffer){    uint32_t                      index = 0;    const ble_gatts_evt_write_t * p_evt_write;    p_evt_write = &(p_ble_evt->evt.gatts_evt.params.write);    index            += uint16_encode(p_evt_write->handle, &p_buffer[index]);    p_buffer[index++] = p_evt_write->op;    index            += uint16_encode(p_evt_write->context.srvc_uuid.uuid, &p_buffer[index]);    p_buffer[index++] = p_evt_write->context.srvc_uuid.type;    index            += uint16_encode(p_evt_write->context.char_uuid.uuid, &p_buffer[index]);    p_buffer[index++] = p_evt_write->context.char_uuid.type;    index            += uint16_encode(p_evt_write->context.desc_uuid.uuid, &p_buffer[index]);    p_buffer[index++] = p_evt_write->context.desc_uuid.type;    index            += uint16_encode(p_evt_write->context.srvc_handle, &p_buffer[index]);    index            += uint16_encode(p_evt_write->context.value_handle, &p_buffer[index]);    p_buffer[index++] = p_evt_write->context.type;    index            += uint16_encode(p_evt_write->offset, &p_buffer[index]);    index            += uint16_encode(p_evt_write->len, &p_buffer[index]);    if (p_evt_write->len != 0)    {        memcpy(&(p_buffer[index]), p_evt_write->data, p_evt_write->len);        index += p_evt_write->len;    }    return index;}

/**@brief Function for encoding a PnP ID. * * @param[out]  p_encoded_buffer   Buffer where the encoded data will be written. * @param[in]   p_pnp_id           PnP ID to be encoded. */static void pnp_id_encode(uint8_t * p_encoded_buffer, const ble_dis_pnp_id_t * p_pnp_id){    uint8_t len = 0;    APP_ERROR_CHECK_BOOL(p_pnp_id != NULL);    APP_ERROR_CHECK_BOOL(p_encoded_buffer != NULL);    p_encoded_buffer[len++] = p_pnp_id->vendor_id_source;    len += uint16_encode(p_pnp_id->vendor_id, &p_encoded_buffer[len]);    len += uint16_encode(p_pnp_id->product_id, &p_encoded_buffer[len]);    len += uint16_encode(p_pnp_id->product_version, &p_encoded_buffer[len]);    APP_ERROR_CHECK_BOOL(len == BLE_DIS_PNP_ID_LEN);}

uint32_t ble_gap_appearance_get_rsp_enc(uint32_t               return_code,                                        uint8_t * const        p_buf,                                        uint32_t * const       p_buf_len,                                        uint16_t const * const p_appearance){    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    uint32_t total_len = *p_buf_len;    uint32_t err_code = ser_ble_cmd_rsp_status_code_enc(SD_BLE_GAP_APPEARANCE_GET, return_code,                                                        p_buf, p_buf_len);    if (err_code != NRF_SUCCESS)    {        return err_code;    }    if (return_code != NRF_SUCCESS)    {        return NRF_SUCCESS;    }    SER_ASSERT_NOT_NULL(p_appearance);    uint32_t index = *p_buf_len;    SER_ASSERT_LENGTH_LEQ(index + sizeof (uint16_t), total_len);    index += uint16_encode(*p_appearance, &p_buf[index]);    *p_buf_len = index;    return NRF_SUCCESS;}

static uint32_t appearance_encode(uint8_t * p_encoded_data, uint8_t * p_len){    uint32_t err_code;    uint16_t appearance;    // Check for buffer overflow    if ((*p_len) + 4 > BLE_GAP_ADV_MAX_SIZE)    {        return NRF_ERROR_DATA_SIZE;    }    // Get GAP appearance field    err_code = sd_ble_gap_appearance_get(&appearance);    if (err_code != NRF_SUCCESS)    {        return err_code;    }        // Encode Length, AD Type and Appearance    p_encoded_data[(*p_len)++] = 3;    p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_APPEARANCE;    (*p_len) += uint16_encode(appearance, &p_encoded_data[*p_len]);        return NRF_SUCCESS;}

static uint32_t manuf_specific_data_encode(const ble_advdata_manuf_data_t * p_manuf_sp_data,                                           uint8_t *                        p_encoded_data,                                           uint8_t *                        p_len){    uint8_t data_size = sizeof(uint16_le_t) + p_manuf_sp_data->data.size;        // Check for buffer overflow    if ((*p_len) + 2 + data_size > BLE_GAP_ADV_MAX_SIZE)    {        return NRF_ERROR_DATA_SIZE;    }    // Encode Length and AD Type    p_encoded_data[(*p_len)++] = 1 + data_size;    p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA;        // Encode Company Identifier    (*p_len) += uint16_encode(p_manuf_sp_data->company_identifier, &p_encoded_data[*p_len]);        // Encode additional manufacturer specific data    if (p_manuf_sp_data->data.size > 0)    {        if (p_manuf_sp_data->data.p_data == NULL)        {            return NRF_ERROR_INVALID_PARAM;        }        memcpy(&p_encoded_data[*p_len], p_manuf_sp_data->data.p_data, p_manuf_sp_data->data.size);        (*p_len) += p_manuf_sp_data->data.size;    }        return NRF_SUCCESS;}

void ble_agsensor_create_sensor_read_data_msg(void) {	hvx_p_config_len = 0;	(hvx_p_config_len) += uint16_encode(BLE_AGSENSOR_CONFIGURATION_OPCODE_START_MEASUREMENT, &hvx_encoded_buffer[hvx_p_config_len]);	// Add the sensor count in the sending msg	uint8_t indice = sentek_measurement_get_Indice_toSend();	hvx_encoded_buffer[hvx_p_config_len] = indice;	hvx_p_config_len ++;	// Add the time-stamp value in the sending msg	(hvx_p_config_len) += uint32_encode(sentek_get_measurement(indice, SENTEK_RESULT_DATA_TIMESTAMP), &hvx_encoded_buffer[hvx_p_config_len]);	// Add the temperature value in the sending msg	uint32_t temp = sentek_get_measurement_float(indice, SENTEK_RESULT_DATA_TEMPERATURE);//	char* tempfloat = &temp;	(hvx_p_config_len) += uint32_encode(temp, &hvx_encoded_buffer[hvx_p_config_len]);	// Add the moisture value in the sending msg//	(hvx_p_config_len) += uint32_encode(sentek_get_measurement(indice, SENTEK_RESULT_DATA_MOISTURE), &hvx_encoded_buffer[hvx_p_config_len]);	(hvx_p_config_len) += uint32_encode(sentek_get_measurement_float(indice, SENTEK_RESULT_DATA_MOISTURE), &hvx_encoded_buffer[hvx_p_config_len]);	// Add the salinity value in the sending msg//	(hvx_p_config_len) += uint32_encode(sentek_get_measurement(indice, SENTEK_RESULT_DATA_SALINITY), &hvx_encoded_buffer[hvx_p_config_len]);	(hvx_p_config_len) += uint32_encode(sentek_get_measurement_float(indice, SENTEK_RESULT_DATA_SALINITY), &hvx_encoded_buffer[hvx_p_config_len]);}

uint32_t ant_channel_id_set_req_enc(uint8_t          channel,                                    uint16_t         device_number,                                    uint8_t          device_type,                                    uint8_t          transmit_type,                                    uint8_t * const  p_buf,                                    uint32_t * const p_buf_len){    uint32_t index    = 0;    uint32_t err_code = NRF_SUCCESS;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    p_buf[index++] = SVC_ANT_CHANNEL_ID_SET;    p_buf[index++] = channel;    index += uint16_encode(device_number,  &p_buf[index]);    p_buf[index++] = device_type;    p_buf[index++] = transmit_type;    SER_ASSERT_LENGTH_LEQ(index, *p_buf_len);    *p_buf_len = index;    return err_code;}

uint32_t ble_gatts_service_add_req_enc(uint8_t                  type,                                       ble_uuid_t const * const p_uuid,                                       uint16_t const * const   p_conn_handle,                                       uint8_t * const          p_buf,                                       uint32_t * const         p_buf_len){    uint32_t index = 0;    SER_ASSERT_NOT_NULL(p_buf);    SER_ASSERT_NOT_NULL(p_buf_len);    SER_ASSERT_LENGTH_LEQ(index + 1 + 1 + 1 + 1, *p_buf_len);    p_buf[index++] = SD_BLE_GATTS_SERVICE_ADD;    p_buf[index++] = type;    p_buf[index++] = (p_uuid != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    if (p_uuid != NULL)    {        SER_ASSERT_LENGTH_LEQ(index + 3, *p_buf_len);        index         += uint16_encode(p_uuid->uuid, &p_buf[index]);        p_buf[index++] = p_uuid->type;    }    SER_ASSERT_LENGTH_LEQ(index + 1, *p_buf_len);    p_buf[index++] = (p_conn_handle != NULL) ? SER_FIELD_PRESENT : SER_FIELD_NOT_PRESENT;    *p_buf_len = index;    return NRF_SUCCESS;}

void ant_bpwr_page_16_encode(uint8_t                      * p_page_buffer,                             ant_bpwr_page16_data_t const * p_page_data){    ant_bpwr_page16_data_layout_t * p_outcoming_data =        (ant_bpwr_page16_data_layout_t *)p_page_buffer;    p_outcoming_data->update_event_count    = p_page_data->update_event_count;    p_outcoming_data->pedal_power           = p_page_data->pedal_power.byte;    UNUSED_PARAMETER(uint16_encode(p_page_data->accumulated_power,                                   p_outcoming_data->accumulated_power));    UNUSED_PARAMETER(uint16_encode(p_page_data->instantaneous_power,                                   p_outcoming_data->instantaneous_power));    page16_data_log(p_page_data);}

/**@brief Function for decoding a command packet with RPC_SD_BLE_GAP_APPEARANCE_GET opcode. * * This function will decode the command, call the BLE Stack API, and also send command response * to the peer through the the transport layer. * * @param[in] p_command         The encoded structure that needs to be decoded and passed on *                              to the BLE Stack API. * @param[in] command_len       The length of the encoded command read from transport layer. * * @retval NRF_SUCCESS               If the decoding of the command was successful, the SoftDevice *                                   API was called, and the command response was sent to peer, *                                   otherwise an error code. * @retval NRF_ERROR_INVALID_LENGTH  If the content length of the packet is not conforming to the *                                   codec specification. */static uint32_t gap_appearance_get_handle(uint8_t const * const p_command, uint32_t command_len){    uint8_t  resp_data[sizeof(uint16_t)];    uint16_t appearance;    uint32_t err_code;    uint32_t   index        = 0;    uint8_t    out_index    = 0;    uint16_t * p_appearance = &appearance;        // If appearance field is present.    if (p_command[index++] == RPC_BLE_FIELD_NOT_PRESENT)    {        p_appearance = NULL;    }        RPC_DECODER_LENGTH_CHECK(command_len, index, SD_BLE_GAP_APPEARANCE_GET);    err_code  = sd_ble_gap_appearance_get(p_appearance);        if (err_code == NRF_SUCCESS)    {        out_index += uint16_encode(*p_appearance, resp_data);        return ble_rpc_cmd_resp_data_send(SD_BLE_GAP_APPEARANCE_GET,                                          err_code,                                          resp_data,                                          out_index);    }    else    {        return ble_rpc_cmd_resp_send(SD_BLE_GAP_APPEARANCE_GET, err_code);    }       }

uint32_t ser_phy_tx_pkt_send(const uint8_t * p_buffer, uint16_t num_of_bytes){    if (p_buffer == NULL)    {        return NRF_ERROR_NULL;    }    else if (num_of_bytes == 0)    {        return NRF_ERROR_INVALID_PARAM;    }    //Check if there is no ongoing transmission at the moment    if ((mp_tx_stream == NULL) && (m_tx_stream_length == 0) && (m_tx_stream_index == 0))    {        (void) uint16_encode(num_of_bytes, m_tx_length_buf);        mp_tx_stream       = (uint8_t *)p_buffer;        m_tx_stream_length = num_of_bytes + SER_PHY_HEADER_SIZE;        //Call tx procedure to start transmission of a packet        ser_phy_uart_tx();    }    else    {        return NRF_ERROR_BUSY;    }    return NRF_SUCCESS;}

void ant_common_page_80_encode(uint8_t                                 * p_page_buffer,                               volatile ant_common_page80_data_t const * p_page_data){    ant_common_page80_data_layout_t * p_outcoming_data =        (ant_common_page80_data_layout_t *)p_page_buffer;    memset(p_page_buffer, UINT8_MAX, sizeof (p_outcoming_data->reserved));    p_outcoming_data->hw_revision = p_page_data->hw_revision;    UNUSED_PARAMETER(uint16_encode(p_page_data->manufacturer_id,                                   p_outcoming_data->manufacturer_id));    UNUSED_PARAMETER(uint16_encode(p_page_data->model_number, p_outcoming_data->model_number));    page80_data_log(p_page_data);}