/* * Copyright (C) 2014 BlueKitchen GmbH * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * 3. Neither the name of the copyright holders nor the names of * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * 4. Any redistribution, use, or modification is done solely for * personal benefit and not for any commercial purpose or for * monetary gain. * * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * Please inquire about commercial licensing options at * contact@bluekitchen-gmbh.com * */ #define __BTSTACK_FILE__ "cycling_power_server_test.c" #include #include #include #include #include "cycling_power_server_test.h" #include "btstack_config.h" #include "btstack.h" // https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.characteristic.gap.appearance.xml // cycling / cycling power sensor static const uint16_t appearance = (18 << 6) | 4; static uint16_t con_handle; static uint16_t event_time_s = 0; static uint16_t force_magnitude_newton = 0; static uint16_t torque_magnitude_newton_m = 0; static uint16_t angle_deg = 0; static uint8_t broadcast_adv[31]; static uint16_t adv_int_min = 0x0030; static uint16_t adv_int_max = 0x0030; static btstack_packet_callback_registration_t hci_event_callback_registration; const uint8_t adv_data[] = { // Flags general discoverable, BR/EDR not supported 0x02, BLUETOOTH_DATA_TYPE_FLAGS, 0x06, // Name 0x0E, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 'C', 'y', 'c', 'l', 'i', 'n', 'g', '_', 'P', 'o', 'w', 'e', 'r', // 16-bit Service UUIDs 0x03, BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS, ORG_BLUETOOTH_SERVICE_CYCLING_POWER & 0xff, ORG_BLUETOOTH_SERVICE_CYCLING_POWER >> 8, // Appearance 3, BLUETOOTH_DATA_TYPE_APPEARANCE, appearance & 0xff, appearance >> 8 }; const uint8_t adv_data_len = sizeof(adv_data); static cycling_power_sensor_location_t supported_sensor_locations[] = { CP_SENSOR_LOCATION_TOP_OF_SHOE, CP_SENSOR_LOCATION_IN_SHOE, CP_SENSOR_LOCATION_HIP, CP_SENSOR_LOCATION_FRONT_WHEEL, CP_SENSOR_LOCATION_LEFT_CRANK, CP_SENSOR_LOCATION_RIGHT_CRANK }; static uint16_t num_supported_sensor_locations = 6; static gatt_date_time_t calibration_date = {2018, 1, 1, 13, 40, 50}; static cycling_power_sensor_measurement_context_t measurement_type; static int enhanced_calibration = 0; static uint16_t manufacturer_company_id = 0xf0f0; // SIG assigned numbers static uint8_t numb_manufacturer_specific_data = CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE; static uint8_t manufacturer_specific_data[] = { 0x11, 0x11, 0x11, 0x11, 0x22, 0x22, 0x22, 0x22, 0x33, 0x33, 0x33, 0x33, 0x44, 0x44, 0x44, 0x44 }; #ifdef HAVE_BTSTACK_STDIN // static char * measurement_flag_str[] = { // "1 Pedal Power Balance", // "2 Pedal Power Balance Reference", // Unknown/Left // "2 Accumulated Torque", // Wheel Based/Crank // "2 Accumulated Torque Source", // Wheel Based/Crank // "4 2 Wheel Revolution Data", // "2 2 Crank Revolution Data", // "2 2 Extreme Force Magnitudes", // "2 2 Extreme Torque Magnitudes", // "3 Extreme Angles", // "2 Top Dead Spot Angle", // "2 Bottom Dead Spot Angle", // "2 Accumulated Energy", // "Offset Compensation Indicator" // }; // static char buffer[80]; // static char * measurement_flag2str(cycling_power_measurement_flag_t flag, uint8_t value){ // if (flag >= CP_MEASUREMENT_FLAG_RESERVED) return "Reserved"; // strcpy(buffer, measurement_flag_str[flag]); // int pos = strlen(measurement_flag_str[flag]); // // printf(" copy %d\n", pos); // switch (flag){ // case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_REFERENCE: // if (value == 0){ // strcpy(buffer + pos, ": Unknown"); // } else { // strcpy(buffer + pos, ": Left"); // } // break; // case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT: // if (value == 0){ // strcpy(buffer + pos, ": Wheel Based"); // } else { // strcpy(buffer + pos, ": Crank Based"); // } // break; // case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_SOURCE: // if (value == 0){ // strcpy(buffer + pos, ": Wheel Based"); // } else { // strcpy(buffer + pos, ": Crank Based"); // } // break; // default: // if (value == 0){ // strcpy(buffer + pos, ": NOT SUPPORTED"); // } // break; // } // return &buffer[0]; // } static void dump_feature_flags(uint32_t feature_flags){ int i; printf("feature flags: \n"); for (i = 0; i < CP_FEATURE_FLAG_RESERVED; i++){ printf("%02d ", i); } printf("\n"); for (i = 0; i < CP_FEATURE_FLAG_RESERVED; i++){ uint8_t value = (feature_flags & (1 << i)) != 0; printf("%2d ", value); } printf("\n"); } static void dump_measurement_flags(uint16_t measurement_flags){ int i; printf("measurement flags: \n"); // for (i = 0; i < CP_MEASUREMENT_FLAG_RESERVED; i++){ // printf("%02d ", i); // } // printf("\n"); for (i = 0; i < CP_MEASUREMENT_FLAG_RESERVED; i++){ uint8_t value = (measurement_flags & (1 << i)) != 0; printf("%2d ", value); } printf("\n"); } static void show_usage(void){ bd_addr_t iut_address; gap_local_bd_addr(iut_address); printf("\n--- Bluetooth Cycling Power Server Test Console %s ---\n", bd_addr_to_str(iut_address)); printf("u - push update\n"); printf("\n"); printf("t - add positive torque\n"); printf("T - add negative torque\n"); printf("w - add wheel revolution\n"); printf("c - add crank revolution\n"); printf("e - add energy\n"); printf("\n"); printf("p - set instantaneous power\n"); printf("b - set power balance percentage\n"); printf("m - set force magnitude\n"); printf("M - set torque magnitude\n"); printf("a - set angle\n"); printf("x - set top dead spot angle\n"); printf("y - set bottom dead spot angle\n"); printf("R - reset values\n"); printf("z - stop calibration\n"); printf("Z - incorrect calibration position\n"); printf("Y - Invalid param\n"); printf("X - start calibration\n"); printf("\n"); printf("Ctrl-c - exit\n"); printf("---\n"); } static void cps_reset_values(void){ event_time_s = 0; force_magnitude_newton = 0; torque_magnitude_newton_m = 0; angle_deg = 0; } static void stdin_process(char cmd){ switch (cmd){ case 'C': printf("reset all values\n"); cps_reset_values(); break; case 'u':{ printf("push update\n"); cycling_power_service_server_update_values(); event_time_s++; break; } case 'r': printf("add positive torque\n"); cycling_power_service_server_add_torque(100); break; case 'R': printf("add negative torque\n"); cycling_power_service_server_add_torque(-100); break; case 'w': printf("add wheel revolution\n"); cycling_power_service_server_add_wheel_revolution(10, event_time_s); break; case 'W': printf("reverse wheel revolution\n"); cycling_power_service_server_add_wheel_revolution(-10, event_time_s); break; case 'c': printf("add crank revolution\n"); cycling_power_service_server_add_crank_revolution(10, event_time_s); break; case 'e': printf("add energy\n"); cycling_power_service_add_energy(100); break; case 'p': printf("set instantaneous power\n"); cycling_power_service_server_set_instantaneous_power(100); break; case 'b': printf("set power balance percentage\n"); cycling_power_service_server_set_pedal_power_balance(50); break; case 'm': force_magnitude_newton += 10; printf("set force magnitude\n"); cycling_power_service_server_set_force_magnitude(force_magnitude_newton-5, force_magnitude_newton+5); break; case 'M': torque_magnitude_newton_m += 10; printf("set torque magnitude\n"); cycling_power_service_server_set_torque_magnitude(torque_magnitude_newton_m-5, torque_magnitude_newton_m+5); break; case 'a': angle_deg += 10; printf("set angle\n"); cycling_power_service_server_set_angle(angle_deg-5, angle_deg+5); break; case 'x': printf("set top dead spot angle\n"); cycling_power_service_server_set_top_dead_spot_angle(180); break; case 'y': printf("set bottom dead spot angle\n"); cycling_power_service_server_set_bottom_dead_spot_angle(20); break; case 'Y': printf("Invalid parameter\n"); uint16_t calibrated_value = 0; if (enhanced_calibration){ cycling_power_server_enhanced_calibration_done(3, calibrated_value, manufacturer_company_id, numb_manufacturer_specific_data, manufacturer_specific_data); } else { cycling_power_server_calibration_done(2, calibrated_value); } break; case 'z':{ printf("stop calibration\n"); switch (measurement_type){ case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE: calibrated_value = force_magnitude_newton; break; case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE: calibrated_value = torque_magnitude_newton_m; break; default: printf("wrong measurement type\n"); break; } if (enhanced_calibration){ printf(" enhanced calibration on\n"); cycling_power_server_enhanced_calibration_done(measurement_type, calibrated_value, manufacturer_company_id, numb_manufacturer_specific_data, manufacturer_specific_data); } else { // printf("cycling_power_server_calibration_done, data %d \n", calibrated_value); cycling_power_server_calibration_done(measurement_type, calibrated_value); } break; } case 'Z': printf("stop calibration, incorrect calibration position\n"); if (enhanced_calibration){ printf(" enhanced calibration on\n"); cycling_power_server_enhanced_calibration_done(measurement_type, CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION, manufacturer_company_id, numb_manufacturer_specific_data, manufacturer_specific_data); } else { cycling_power_server_calibration_done(measurement_type, CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION); } break; case 't': printf("disconnect \n"); // gap_advertisements_enable(0); gap_disconnect(con_handle); break; case '\n': case '\r': break; default: show_usage(); break; } } #endif static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ UNUSED(channel); UNUSED(size); int pos; bd_addr_t null_addr; if (packet_type != HCI_EVENT_PACKET) return; switch (hci_event_packet_get_type(packet)){ case HCI_EVENT_LE_META: switch (hci_event_le_meta_get_subevent_code(packet)){ case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: con_handle = hci_subevent_le_connection_complete_get_connection_handle(packet); break; default: break; } break; case HCI_EVENT_GATTSERVICE_META: switch (packet[2]){ case GATTSERVICE_SUBEVENT_CYCLING_POWER_START_CALIBRATION: measurement_type = gattservice_subevent_cycling_power_start_calibration_get_measurement_type(packet); enhanced_calibration = gattservice_subevent_cycling_power_start_calibration_get_is_enhanced(packet); break; case GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_START: printf("start broadcast\n"); // set ADV_NONCONN_IND pos = cycling_power_get_measurement_adv(adv_int_max, &broadcast_adv[0], sizeof(broadcast_adv)); memset(null_addr, 0, 6); gap_advertisements_set_params(adv_int_min, adv_int_max, 0x03, 0, null_addr, 0x07, 0x00); gap_advertisements_set_data(pos, (uint8_t*) broadcast_adv); gap_advertisements_enable(1); break; case GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP: printf("stop broadcast\n"); gap_advertisements_enable(0); break; default: break; } break; default: break; } // uint8_t event = hci_event_packet_get_type(packet); } int btstack_main(void); int btstack_main(void){ hci_event_callback_registration.callback = &packet_handler; hci_add_event_handler(&hci_event_callback_registration); l2cap_init(); // setup le device db le_device_db_init(); // setup SM: Display only sm_init(); // setup ATT server att_server_init(profile_data, NULL, NULL); // setup heart rate service // cycling_power_service_server_init(0x2FFFFF, 0x1F, CP_PEDAL_POWER_BALANCE_REFERENCE_LEFT); uint32_t feature_flags = 0; feature_flags |= (1 << CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED); // feature_flags |= (1 << CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); // feature_flags |= (1 << CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED); // feature_flags |= (1 << CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_ACCUMULATED_ENERGY_SUPPORTED); // feature_flags |= (1 << CP_FEATURE_FLAG_OFFSET_COMPENSATION_INDICATOR_SUPPORTED); // feature_flags |= (1 << CP_FEATURE_FLAG_OFFSET_COMPENSATION_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED); feature_flags |= (CP_SENSOR_MEASUREMENT_CONTEXT_FORCE << CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT); // feature_flags |= (CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE << CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT); // feature_flags |= (1 << CP_FEATURE_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_FACTORY_CALIBRATION_DATE_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_OFFSET_COMPENSATION_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_ENHANCED_OFFSET_COMPENSATION_SUPPORTED); feature_flags |= (1 << CP_FEATURE_FLAG_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT_MASKING_SUPPORTED); printf(" num_supported_sensor_locations %lu\n", sizeof(num_supported_sensor_locations)); cycling_power_service_server_init(feature_flags, CP_PEDAL_POWER_BALANCE_REFERENCE_LEFT, CP_TORQUE_SOURCE_WHEEL, &supported_sensor_locations[0], num_supported_sensor_locations, supported_sensor_locations[0]); cycling_power_service_server_packet_handler(packet_handler); cycling_power_service_server_set_factory_calibration_date(calibration_date); uint16_t measurement_flags = cycling_power_service_measurement_flags(); dump_feature_flags(feature_flags); dump_measurement_flags(measurement_flags); // dump_measurement_flags_as_str(measurement_flags); cycling_power_service_server_add_torque(100); cycling_power_service_server_add_torque(-100); cycling_power_service_server_add_wheel_revolution(10, event_time_s); cycling_power_service_server_add_crank_revolution(10, event_time_s); cycling_power_service_add_energy(100); cycling_power_service_server_set_instantaneous_power(100); cycling_power_service_server_set_pedal_power_balance(50); force_magnitude_newton += 10; cycling_power_service_server_set_force_magnitude(force_magnitude_newton-5, force_magnitude_newton+5); torque_magnitude_newton_m += 10; cycling_power_service_server_set_torque_magnitude(torque_magnitude_newton_m-5, torque_magnitude_newton_m+5); angle_deg += 10; cycling_power_service_server_set_angle(angle_deg-5, angle_deg+5); cycling_power_service_server_set_top_dead_spot_angle(180); cycling_power_service_server_set_bottom_dead_spot_angle(20); cycling_power_service_add_energy(100); int16_t values[] = {12, -50, 100}; cycling_power_service_server_set_torque_magnitude_values(3, values); cycling_power_service_server_set_force_magnitude_values(3, values); cycling_power_service_server_set_instantaneous_measurement_direction(CP_INSTANTANEOUS_MEASUREMENT_DIRECTION_TANGENTIAL_COMPONENT); // cycling_power_service_server_set_first_crank_measurement_angle(first_crank_measurement_angle_deg); // setup advertisements uint8_t adv_type = 0; // AFV_IND bd_addr_t null_addr; memset(null_addr, 0, 6); gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00); gap_advertisements_set_data(adv_data_len, (uint8_t*) adv_data); gap_advertisements_enable(1); #ifdef HAVE_BTSTACK_STDIN btstack_stdin_setup(stdin_process); #endif // turn on! hci_power_control(HCI_POWER_ON); return 0; } /* EXAMPLE_END */