1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN 24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define BTSTACK_FILE__ "cycling_power_service_server.c" 39 40 41 #include "bluetooth.h" 42 #include "btstack_defines.h" 43 #include "bluetooth_data_types.h" 44 #include "btstack_event.h" 45 #include "ble/att_db.h" 46 #include "ble/att_server.h" 47 #include "btstack_util.h" 48 #include "bluetooth_gatt.h" 49 #include "btstack_debug.h" 50 #include "l2cap.h" 51 #include "hci.h" 52 53 #include "ble/gatt-service/cycling_power_service_server.h" 54 55 #define CYCLING_POWER_MAX_BROACAST_MSG_SIZE 31 56 #define CONTROL_POINT_PROCEDURE_TIMEOUT_MS 30 57 #define CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED 0xFFFF 58 59 typedef enum { 60 CP_MASK_BIT_PEDAL_POWER_BALANCE = 0, 61 CP_MASK_BIT_ACCUMULATED_TORQUE, 62 CP_MASK_BIT_WHEEL_REVOLUTION_DATA, 63 CP_MASK_BIT_CRANK_REVOLUTION_DATA, 64 CP_MASK_BIT_EXTREME_MAGNITUDES, 65 CP_MASK_BIT_EXTREME_ANGLES, 66 CP_MASK_BIT_TOP_DEAD_SPOT_ANGLE, 67 CP_MASK_BIT_BOTTOM_DEAD_SPOT_ANGLE, 68 CP_MASK_BIT_ACCUMULATED_ENERGY, 69 CP_MASK_BIT_RESERVED 70 } cycling_power_mask_bit_t; 71 72 typedef enum { 73 CP_OPCODE_IDLE = 0, 74 CP_OPCODE_SET_CUMULATIVE_VALUE, 75 CP_OPCODE_UPDATE_SENSOR_LOCATION, 76 CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS, 77 CP_OPCODE_SET_CRANK_LENGTH, 78 CP_OPCODE_REQUEST_CRANK_LENGTH, 79 CP_OPCODE_SET_CHAIN_LENGTH, 80 CP_OPCODE_REQUEST_CHAIN_LENGTH, 81 CP_OPCODE_SET_CHAIN_WEIGHT, 82 CP_OPCODE_REQUEST_CHAIN_WEIGHT, 83 CP_OPCODE_SET_SPAN_LENGTH, 84 CP_OPCODE_REQUEST_SPAN_LENGTH, 85 CP_OPCODE_START_OFFSET_COMPENSATION, 86 CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT, 87 CP_OPCODE_REQUEST_SAMPLING_RATE, 88 CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE, 89 CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION, 90 CP_OPCODE_RESPONSE_CODE = 32 91 } cycling_power_opcode_t; 92 93 typedef enum { 94 CP_RESPONSE_VALUE_SUCCESS = 1, 95 CP_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED, 96 CP_RESPONSE_VALUE_INVALID_PARAMETER, 97 CP_RESPONSE_VALUE_OPERATION_FAILED, 98 CP_RESPONSE_VALUE_NOT_AVAILABLE, 99 CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE 100 } cycling_power_response_value_t; 101 102 typedef enum { 103 CP_CONNECTION_INTERVAL_STATUS_NONE = 0, 104 CP_CONNECTION_INTERVAL_STATUS_RECEIVED, 105 CP_CONNECTION_INTERVAL_STATUS_ACCEPTED, 106 CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE, 107 CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE, 108 CP_CONNECTION_INTERVAL_STATUS_REJECTED 109 } cycling_power_con_interval_status_t; 110 111 typedef struct { 112 hci_con_handle_t con_handle; 113 // GATT connection management 114 uint16_t con_interval; 115 uint16_t con_interval_min; 116 uint16_t con_interval_max; 117 cycling_power_con_interval_status_t con_interval_status; 118 119 // Cycling Power Measurement 120 uint16_t measurement_value_handle; 121 int16_t instantaneous_power_W; 122 123 cycling_power_pedal_power_balance_reference_t pedal_power_balance_reference; 124 uint8_t pedal_power_balance_percentage; // percentage, resolution 1/2, 125 // If the sensor provides the power balance referenced to the left pedal, 126 // the power balance is calculated as [LeftPower/(LeftPower + RightPower)]*100 in units of percent 127 128 cycling_power_torque_source_t torque_source; 129 uint16_t accumulated_torque_Nm; // newton-meters, resolution 1/32, 130 // The Accumulated Torque value may decrease 131 // wheel revolution data: 132 uint32_t cumulative_wheel_revolutions; // CANNOT roll over 133 uint16_t last_wheel_event_time_s; // seconds, resolution 1/2048 134 // crank revolution data: 135 uint16_t cumulative_crank_revolutions; 136 uint16_t last_crank_event_time_s; // seconds, resolution 1/1024 137 // extreme force magnitudes 138 int16_t maximum_force_magnitude_N; 139 int16_t minimum_force_magnitude_N; 140 int16_t maximum_torque_magnitude_Nm; // newton-meters, resolution 1/32 141 int16_t minimum_torque_magnitude_Nm; // newton-meters, resolution 1/32 142 // extreme angles 143 uint16_t maximum_angle_degree; // 12bit, degrees 144 uint16_t minimum_angle_degree; // 12bit, degrees, concatenated with previous into 3 octets 145 // i.e. if the Maximum Angle is 0xABC and the Minimum Angle is 0x123, the transmitted value is 0x123ABC. 146 uint16_t top_dead_spot_angle_degree; 147 uint16_t bottom_dead_spot_angle_degree; // The Bottom Dead Spot Angle field represents the crank angle when the value of the Instantaneous Power value becomes negative. 148 uint16_t accumulated_energy_kJ; // kilojoules; CANNOT roll over 149 150 // uint8_t offset_compensation; 151 152 // CP Measurement Notification (Client Characteristic Configuration) 153 uint16_t measurement_client_configuration_descriptor_handle; 154 uint16_t measurement_client_configuration_descriptor_notify; 155 btstack_context_callback_registration_t measurement_notify_callback; 156 157 // CP Measurement Broadcast (Server Characteristic Configuration) 158 uint16_t measurement_server_configuration_descriptor_handle; 159 uint16_t measurement_server_configuration_descriptor_broadcast; 160 btstack_context_callback_registration_t measurement_broadcast_callback; 161 162 // Cycling Power Feature 163 uint16_t feature_value_handle; 164 uint32_t feature_flags; // see cycling_power_feature_flag_t 165 uint16_t masked_measurement_flags; 166 uint16_t default_measurement_flags; 167 168 // Sensor Location 169 uint16_t sensor_location_value_handle; 170 cycling_power_sensor_location_t sensor_location; // see cycling_power_sensor_location_t 171 cycling_power_sensor_location_t * supported_sensor_locations; 172 uint16_t num_supported_sensor_locations; 173 uint16_t crank_length_mm; // resolution 1/2 mm 174 uint16_t chain_length_mm; // resolution 1 mm 175 uint16_t chain_weight_g; // resolution 1 gram 176 uint16_t span_length_mm; // resolution 1 mm 177 178 gatt_date_time_t factory_calibration_date; 179 180 uint8_t sampling_rate_Hz; // resolution 1 Herz 181 182 int16_t current_force_magnitude_N; 183 int16_t current_torque_magnitude_Nm; // newton-meters, resolution 1/32 184 uint16_t manufacturer_company_id; 185 uint8_t num_manufacturer_specific_data; 186 uint8_t * manufacturer_specific_data; 187 188 // Cycling Power Vector 189 uint16_t vector_value_handle; 190 uint16_t vector_cumulative_crank_revolutions; 191 uint16_t vector_last_crank_event_time_s; // seconds, resolution 1/1024 192 uint16_t vector_first_crank_measurement_angle_degree; 193 int16_t * vector_instantaneous_force_magnitude_N_array; // newton 194 int force_magnitude_count; 195 int16_t * vector_instantaneous_torque_magnitude_Nm_array; // newton-meter, resolution 1/32 196 int torque_magnitude_count; 197 cycling_power_instantaneous_measurement_direction_t vector_instantaneous_measurement_direction; 198 199 // CP Vector Notification (Client Characteristic Configuration) 200 uint16_t vector_client_configuration_descriptor_handle; 201 uint16_t vector_client_configuration_descriptor_notify; 202 btstack_context_callback_registration_t vector_notify_callback; 203 204 // CP Control Point 205 uint16_t control_point_value_handle; 206 // CP Control Point Indication (Client Characteristic Configuration) 207 uint16_t control_point_client_configuration_descriptor_handle; 208 uint16_t control_point_client_configuration_descriptor_indicate; 209 btstack_context_callback_registration_t control_point_indicate_callback; 210 211 cycling_power_opcode_t request_opcode; 212 cycling_power_response_value_t response_value; 213 214 btstack_packet_handler_t calibration_callback; 215 uint8_t w4_indication_complete; 216 } cycling_power_t; 217 218 static att_service_handler_t cycling_power_service; 219 static cycling_power_t cycling_power; 220 static btstack_packet_callback_registration_t hci_event_callback_registration; 221 static btstack_packet_callback_registration_t l2cap_event_callback_registration; 222 223 static uint16_t cycling_power_service_read_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){ 224 UNUSED(con_handle); 225 UNUSED(attribute_handle); 226 UNUSED(offset); 227 cycling_power_t * instance = &cycling_power; 228 229 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 230 if (buffer && (buffer_size >= 2u)){ 231 little_endian_store_16(buffer, 0, instance->measurement_client_configuration_descriptor_notify); 232 } 233 return 2; 234 } 235 236 if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){ 237 if (buffer && (buffer_size >= 2u)){ 238 little_endian_store_16(buffer, 0, instance->measurement_server_configuration_descriptor_broadcast); 239 } 240 return 2; 241 } 242 243 if (attribute_handle == instance->vector_client_configuration_descriptor_handle){ 244 if (buffer && (buffer_size >= 2u)){ 245 little_endian_store_16(buffer, 0, instance->vector_client_configuration_descriptor_notify); 246 } 247 return 2; 248 } 249 250 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 251 if (buffer && (buffer_size >= 2u)){ 252 little_endian_store_16(buffer, 0, instance->control_point_client_configuration_descriptor_indicate); 253 } 254 return 2; 255 } 256 257 if (attribute_handle == instance->feature_value_handle){ 258 if (buffer && (buffer_size >= 4u)){ 259 little_endian_store_32(buffer, 0, instance->feature_flags); 260 } 261 return 4; 262 } 263 264 if (attribute_handle == instance->sensor_location_value_handle){ 265 if (buffer && (buffer_size >= 1u)){ 266 buffer[0] = instance->sensor_location; 267 } 268 return 1; 269 } 270 return 0; 271 } 272 273 static int has_feature(cycling_power_feature_flag_t feature){ 274 cycling_power_t * instance = &cycling_power; 275 return (instance->feature_flags & (1u << feature)) != 0u; 276 } 277 278 static int cycling_power_vector_instantaneous_measurement_direction(void){ 279 cycling_power_t * instance = &cycling_power; 280 return instance->vector_instantaneous_measurement_direction; 281 } 282 283 static uint16_t cycling_power_service_default_measurement_flags(void){ 284 cycling_power_t * instance = &cycling_power; 285 uint16_t measurement_flags = 0; 286 uint8_t flag[] = { 287 (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED), 288 (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED) && instance->pedal_power_balance_reference, 289 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED), 290 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED) && instance->torque_source, 291 (uint8_t) has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED), 292 (uint8_t) has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED), 293 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE), 294 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE), 295 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED), 296 (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED), 297 (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED), 298 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_ENERGY_SUPPORTED), 299 (uint8_t) has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_INDICATOR_SUPPORTED) 300 }; 301 302 int i; 303 for (i = CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT; i <= CP_MEASUREMENT_FLAG_OFFSET_COMPENSATION_INDICATOR; i++){ 304 measurement_flags |= flag[i] << i; 305 } 306 307 return measurement_flags; 308 } 309 310 static uint16_t cycling_power_service_get_measurement_flags(cycling_power_t * instance){ 311 if (!instance) return 0; 312 if (instance->masked_measurement_flags != CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){ 313 return instance->masked_measurement_flags; 314 } 315 if (instance->default_measurement_flags == CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){ 316 instance->default_measurement_flags = cycling_power_service_default_measurement_flags(); 317 } 318 return instance->default_measurement_flags; 319 } 320 321 322 uint16_t cycling_power_service_measurement_flags(void){ 323 cycling_power_t * instance = &cycling_power; 324 return cycling_power_service_get_measurement_flags(instance); 325 } 326 327 uint8_t cycling_power_service_vector_flags(void){ 328 uint8_t vector_flags = 0; 329 uint8_t flag[] = { 330 (uint8_t )has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED), 331 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED), 332 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE), 333 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE), 334 (uint8_t )has_feature(CP_FEATURE_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION_SUPPORTED) && cycling_power_vector_instantaneous_measurement_direction() 335 }; 336 337 int i; 338 for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){ 339 vector_flags |= flag[i] << i; 340 } 341 return vector_flags; 342 } 343 344 static void cycling_power_service_vector_can_send_now(void * context){ 345 cycling_power_t * instance = (cycling_power_t *) context; 346 if (!instance){ 347 log_error("cycling_power_service_measurement_can_send_now: instance is null"); 348 return; 349 } 350 uint8_t value[50]; 351 uint8_t vector_flags = cycling_power_service_vector_flags(); 352 int pos = 0; 353 354 value[pos++] = vector_flags; 355 int i; 356 for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){ 357 if ((vector_flags & (1u << i)) == 0u) continue; 358 switch ((cycling_power_vector_flag_t) i){ 359 case CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 360 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 361 pos += 2; 362 little_endian_store_16(value, pos, instance->last_crank_event_time_s); 363 pos += 2; 364 break; 365 case CP_VECTOR_FLAG_INSTANTANEOUS_FORCE_MAGNITUDE_ARRAY_PRESENT:{ 366 uint16_t att_mtu = att_server_get_mtu(instance->con_handle); 367 uint16_t bytes_left = 0; 368 if (att_mtu > (pos + 3u)){ 369 bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos); 370 } 371 while ((bytes_left > 2u) && instance->force_magnitude_count){ 372 little_endian_store_16(value, pos, instance->vector_instantaneous_force_magnitude_N_array[0]); 373 pos += 2; 374 bytes_left -= 2u; 375 instance->vector_instantaneous_force_magnitude_N_array++; 376 instance->force_magnitude_count--; 377 } 378 break; 379 } 380 case CP_VECTOR_FLAG_INSTANTANEOUS_TORQUE_MAGNITUDE_ARRAY_PRESENT:{ 381 uint16_t att_mtu = att_server_get_mtu(instance->con_handle); 382 uint16_t bytes_left = 0; 383 if (att_mtu > (pos + 3u)){ 384 bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos); 385 } 386 387 while ((bytes_left > 2u) && instance->torque_magnitude_count){ 388 little_endian_store_16(value, pos, instance->vector_instantaneous_torque_magnitude_Nm_array[0]); 389 pos += 2; 390 bytes_left -= 2u; 391 instance->vector_instantaneous_torque_magnitude_Nm_array++; 392 instance->torque_magnitude_count--; 393 } 394 break; 395 } 396 case CP_VECTOR_FLAG_FIRST_CRANK_MEASUREMENT_ANGLE_PRESENT: 397 little_endian_store_16(value, pos, instance->vector_first_crank_measurement_angle_degree); 398 pos += 2; 399 break; 400 case CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION: 401 break; 402 default: 403 break; 404 } 405 } 406 407 att_server_notify(instance->con_handle, instance->vector_value_handle, &value[0], pos); 408 } 409 410 static int cycling_power_measurement_flag_value_size(cycling_power_measurement_flag_t flag){ 411 switch (flag){ 412 case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT: 413 return 1; 414 case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT: 415 return 6; 416 case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 417 case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT: 418 case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT: 419 return 4; 420 case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT: 421 return 3; 422 case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT: 423 case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT: 424 case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT: 425 case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT: 426 return 2; 427 default: 428 return 0; 429 } 430 } 431 432 static int cycling_power_store_measurement_flag_value(cycling_power_t * instance, cycling_power_measurement_flag_t flag, uint8_t * value){ 433 if (!instance) return 0; 434 435 int pos = 0; 436 switch (flag){ 437 case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT: 438 value[pos++] = instance->pedal_power_balance_percentage; 439 break; 440 case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT: 441 little_endian_store_16(value, pos, instance->accumulated_torque_Nm); 442 pos += 2; 443 break; 444 case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT: 445 little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions); 446 pos += 4; 447 little_endian_store_16(value, pos, instance->last_wheel_event_time_s); 448 pos += 2; 449 break; 450 case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 451 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 452 pos += 2; 453 little_endian_store_16(value, pos, instance->last_crank_event_time_s); 454 pos += 2; 455 break; 456 case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT: 457 little_endian_store_16(value, pos, (uint16_t)instance->maximum_force_magnitude_N); 458 pos += 2; 459 little_endian_store_16(value, pos, (uint16_t)instance->minimum_force_magnitude_N); 460 pos += 2; 461 break; 462 case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT: 463 little_endian_store_16(value, pos, (uint16_t)instance->maximum_torque_magnitude_Nm); 464 pos += 2; 465 little_endian_store_16(value, pos, (uint16_t)instance->minimum_torque_magnitude_Nm); 466 pos += 2; 467 break; 468 case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT: 469 little_endian_store_24(value, pos, (instance->maximum_angle_degree << 12) | instance->minimum_angle_degree); 470 pos += 3; 471 break; 472 case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT: 473 little_endian_store_16(value, pos, (uint16_t)instance->top_dead_spot_angle_degree); 474 pos += 2; 475 break; 476 case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT: 477 little_endian_store_16(value, pos, (uint16_t)instance->bottom_dead_spot_angle_degree); 478 pos += 2; 479 break; 480 case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT: 481 little_endian_store_16(value, pos, (uint16_t)instance->accumulated_energy_kJ); 482 pos += 2; 483 break; 484 default: 485 break; 486 } 487 return pos; 488 } 489 490 491 static int cycling_power_store_measurement(cycling_power_t * instance, uint8_t * value, uint16_t max_value_size){ 492 if (max_value_size < 4u) return 0u; 493 if (!instance) return 0; 494 495 uint16_t measurement_flags = cycling_power_service_get_measurement_flags(instance); 496 int pos = 0; 497 little_endian_store_16(value, 0, measurement_flags); 498 pos += 2; 499 little_endian_store_16(value, 2, instance->instantaneous_power_W); 500 pos += 2; 501 int flag_index; 502 uint16_t bytes_left = max_value_size - pos; 503 for (flag_index = 0; flag_index < CP_MEASUREMENT_FLAG_RESERVED; flag_index++){ 504 if ((measurement_flags & (1u << flag_index)) == 0u) continue; 505 cycling_power_measurement_flag_t flag = (cycling_power_measurement_flag_t) flag_index; 506 uint16_t value_size = cycling_power_measurement_flag_value_size(flag); 507 if (value_size > bytes_left ) return pos; 508 cycling_power_store_measurement_flag_value(instance, flag, &value[pos]); 509 pos += value_size; 510 bytes_left -= value_size; 511 } 512 return pos; 513 } 514 515 int cycling_power_get_measurement_adv(uint16_t adv_interval, uint8_t * adv_buffer, uint16_t adv_size){ 516 if (adv_size < 12u) return 0u; 517 cycling_power_t * instance = &cycling_power; 518 int pos = 0; 519 // adv flags 520 adv_buffer[pos++] = 2; 521 adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_FLAGS; 522 adv_buffer[pos++] = 0x4; 523 524 // adv interval 525 adv_buffer[pos++] = 3; 526 adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_ADVERTISING_INTERVAL; 527 little_endian_store_16(adv_buffer, pos, adv_interval); 528 pos += 2; 529 // 530 int value_len = cycling_power_store_measurement(instance, &adv_buffer[pos + 4], CYCLING_POWER_MAX_BROACAST_MSG_SIZE - (pos + 4)); 531 adv_buffer[pos++] = 3 + value_len; 532 adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID; 533 little_endian_store_16(adv_buffer, pos, ORG_BLUETOOTH_SERVICE_CYCLING_POWER); 534 pos += 2; 535 // value data already in place cycling_power_get_measurement 536 pos += value_len; 537 // set ADV_NONCONN_IND 538 return pos; 539 } 540 541 static void cycling_power_service_broadcast_can_send_now(void * context){ 542 cycling_power_t * instance = (cycling_power_t *) context; 543 if (!instance){ 544 log_error("cycling_power_service_broadcast_can_send_now: instance is null"); 545 return; 546 } 547 uint8_t value[CYCLING_POWER_MAX_BROACAST_MSG_SIZE]; 548 int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value)); 549 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 550 } 551 552 static void cycling_power_service_measurement_can_send_now(void * context){ 553 cycling_power_t * instance = (cycling_power_t *) context; 554 if (!instance){ 555 log_error("cycling_power_service_measurement_can_send_now: instance is null"); 556 return; 557 } 558 uint8_t value[40]; 559 int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value)); 560 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 561 } 562 563 static void cycling_power_service_response_can_send_now(void * context){ 564 cycling_power_t * instance = (cycling_power_t *) context; 565 if (!instance){ 566 log_error("cycling_power_service_response_can_send_now: instance is null"); 567 return; 568 } 569 570 if (instance->response_value == CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){ 571 log_error("cycling_power_service_response_can_send_now: CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE"); 572 return; 573 } 574 575 // use preprocessor instead of btstack_max to get compile-time constant 576 #if (CP_SENSOR_LOCATION_RESERVED > (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5)) 577 #define MAX_RESPONSE_PAYLOAD CP_SENSOR_LOCATION_RESERVED 578 #else 579 #define MAX_RESPONSE_PAYLOAD (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5) 580 #endif 581 582 uint8_t value[3 + MAX_RESPONSE_PAYLOAD]; 583 int pos = 0; 584 value[pos++] = CP_OPCODE_RESPONSE_CODE; 585 value[pos++] = instance->request_opcode; 586 value[pos++] = instance->response_value; 587 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 588 switch (instance->request_opcode){ 589 case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{ 590 int i; 591 for (i=0; i<instance->num_supported_sensor_locations; i++){ 592 value[pos++] = instance->supported_sensor_locations[i]; 593 } 594 break; 595 } 596 case CP_OPCODE_REQUEST_CRANK_LENGTH: 597 little_endian_store_16(value, pos, instance->crank_length_mm); 598 pos += 2; 599 break; 600 case CP_OPCODE_REQUEST_CHAIN_LENGTH: 601 little_endian_store_16(value, pos, instance->chain_length_mm); 602 pos += 2; 603 break; 604 case CP_OPCODE_REQUEST_CHAIN_WEIGHT: 605 little_endian_store_16(value, pos, instance->chain_weight_g); 606 pos += 2; 607 break; 608 case CP_OPCODE_REQUEST_SPAN_LENGTH: 609 little_endian_store_16(value, pos, instance->span_length_mm); 610 pos += 2; 611 break; 612 case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE: 613 little_endian_store_16(value, pos, instance->factory_calibration_date.year); 614 pos += 2; 615 value[pos++] = instance->factory_calibration_date.month; 616 value[pos++] = instance->factory_calibration_date.day; 617 value[pos++] = instance->factory_calibration_date.hours; 618 value[pos++] = instance->factory_calibration_date.minutes; 619 value[pos++] = instance->factory_calibration_date.seconds; 620 break; 621 case CP_OPCODE_REQUEST_SAMPLING_RATE: 622 value[pos++] = instance->sampling_rate_Hz; 623 break; 624 case CP_OPCODE_START_OFFSET_COMPENSATION: 625 case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION:{ 626 uint16_t calibrated_value = 0xffff; 627 if (has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED)){ 628 if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE) { 629 calibrated_value = instance->current_force_magnitude_N; 630 } else if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE){ 631 calibrated_value = instance->current_torque_magnitude_Nm; 632 } 633 } 634 635 if (calibrated_value == CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION){ 636 value[pos++] = (uint8_t) calibrated_value; 637 // do not include manufacturer ID and data 638 break; 639 } else if (calibrated_value == CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS){ 640 value[pos++] = (uint8_t) calibrated_value; 641 } else { 642 little_endian_store_16(value, pos, calibrated_value); 643 pos += 2; 644 } 645 646 if (instance->request_opcode == CP_OPCODE_START_OFFSET_COMPENSATION) break; 647 little_endian_store_16(value, pos, instance->manufacturer_company_id); 648 pos += 2; 649 int data_len = (instance->num_manufacturer_specific_data < CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE) ? instance->num_manufacturer_specific_data : (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE - 1); 650 value[pos++] = data_len; 651 (void)memcpy(&value[pos], 652 instance->manufacturer_specific_data, data_len); 653 pos += data_len; 654 value[pos++] = 0; 655 break; 656 } 657 case CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT: 658 break; 659 default: 660 break; 661 } 662 } 663 uint8_t status = att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos); 664 if (status == ERROR_CODE_SUCCESS){ 665 instance->w4_indication_complete = 1; 666 instance->request_opcode = CP_OPCODE_IDLE; 667 } else { 668 log_error("can_send_now failed 0x%2x", status); 669 } 670 } 671 672 static void cycling_power_service_server_emit_start_calibration(const cycling_power_t *instance, 673 cycling_power_sensor_measurement_context_t measurement_type, 674 bool enhanced) { 675 uint8_t event[7]; 676 int index = 0; 677 event[index++] = HCI_EVENT_GATTSERVICE_META; 678 event[index++] = sizeof(event) - 2u; 679 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_START_CALIBRATION; 680 little_endian_store_16(event, index, instance->con_handle); 681 index += 2; 682 event[index++] = (uint8_t) measurement_type; 683 event[index++] = enhanced ? 1 : 0; 684 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 685 } 686 687 static int cycling_power_service_write_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){ 688 UNUSED(con_handle); 689 UNUSED(offset); 690 UNUSED(buffer_size); 691 int i; 692 cycling_power_sensor_location_t location; 693 cycling_power_t * instance = &cycling_power; 694 695 if (transaction_mode != ATT_TRANSACTION_MODE_NONE){ 696 return 0; 697 } 698 699 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 700 if (buffer_size < 2u){ 701 return ATT_ERROR_INVALID_OFFSET; 702 } 703 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 704 instance->con_handle = con_handle; 705 log_info("cycling_power_service_write_callback: measurement enabled %d", instance->measurement_client_configuration_descriptor_notify); 706 return 0; 707 } 708 709 if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){ 710 if (buffer_size < 2u){ 711 return ATT_ERROR_INVALID_OFFSET; 712 } 713 instance->measurement_server_configuration_descriptor_broadcast = little_endian_read_16(buffer, 0); 714 instance->con_handle = con_handle; 715 uint8_t event[5]; 716 int index = 0; 717 event[index++] = HCI_EVENT_GATTSERVICE_META; 718 event[index++] = sizeof(event) - 2u; 719 720 if (instance->measurement_server_configuration_descriptor_broadcast){ 721 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_START; 722 log_info("cycling_power_service_write_callback: start broadcast"); 723 } else { 724 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP; 725 log_info("cycling_power_service_write_callback: stop broadcast"); 726 } 727 little_endian_store_16(event, index, con_handle); 728 index += 2; 729 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 730 return 0; 731 } 732 733 if (attribute_handle == instance->vector_client_configuration_descriptor_handle){ 734 if (buffer_size < 2u){ 735 return ATT_ERROR_INVALID_OFFSET; 736 } 737 instance->con_handle = con_handle; 738 739 #ifdef ENABLE_ATT_DELAYED_RESPONSE 740 switch (instance->con_interval_status){ 741 case CP_CONNECTION_INTERVAL_STATUS_REJECTED: 742 return CYCLING_POWER_ERROR_CODE_INAPPROPRIATE_CONNECTION_PARAMETERS; 743 744 case CP_CONNECTION_INTERVAL_STATUS_ACCEPTED: 745 case CP_CONNECTION_INTERVAL_STATUS_RECEIVED: 746 if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){ 747 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE; 748 gap_request_connection_parameter_update(instance->con_handle, instance->con_interval_min, instance->con_interval_max, 4, 100); // 15 ms, 4, 1s 749 return ATT_ERROR_WRITE_RESPONSE_PENDING; 750 } 751 instance->vector_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 752 return 0; 753 default: 754 return ATT_ERROR_WRITE_RESPONSE_PENDING; 755 756 } 757 #endif 758 } 759 760 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 761 if (buffer_size < 2u){ 762 return ATT_ERROR_INVALID_OFFSET; 763 } 764 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 765 instance->con_handle = con_handle; 766 log_info("cycling_power_service_write_callback: indication enabled %d", instance->control_point_client_configuration_descriptor_indicate); 767 return 0; 768 } 769 770 if (attribute_handle == instance->feature_value_handle){ 771 if (buffer_size < 4u){ 772 return ATT_ERROR_INVALID_OFFSET; 773 } 774 instance->feature_flags = little_endian_read_32(buffer, 0); 775 return 0; 776 } 777 778 if (attribute_handle == instance->control_point_value_handle){ 779 if (instance->control_point_client_configuration_descriptor_indicate == 0u) return CYCLING_POWER_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 780 if (instance->w4_indication_complete != 0u){ 781 return CYCLING_POWER_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 782 } 783 int pos = 0; 784 instance->request_opcode = (cycling_power_opcode_t) buffer[pos++]; 785 instance->response_value = CP_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 786 787 switch (instance->request_opcode){ 788 case CP_OPCODE_SET_CUMULATIVE_VALUE: 789 if (!has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED)) break; 790 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, pos); 791 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 792 break; 793 794 case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 795 if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break; 796 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 797 break; 798 799 case CP_OPCODE_UPDATE_SENSOR_LOCATION: 800 if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break; 801 location = (cycling_power_sensor_location_t) buffer[pos]; 802 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 803 for (i=0; i<instance->num_supported_sensor_locations; i++){ 804 if (instance->supported_sensor_locations[i] == location){ 805 instance->sensor_location = location; 806 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 807 break; 808 } 809 } 810 break; 811 812 case CP_OPCODE_REQUEST_CRANK_LENGTH: 813 if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break; 814 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 815 break; 816 case CP_OPCODE_SET_CRANK_LENGTH: 817 if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break; 818 instance->crank_length_mm = little_endian_read_16(buffer, pos); 819 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 820 break; 821 822 case CP_OPCODE_REQUEST_CHAIN_LENGTH: 823 if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 824 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 825 break; 826 case CP_OPCODE_SET_CHAIN_LENGTH: 827 if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 828 instance->chain_length_mm = little_endian_read_16(buffer, pos); 829 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 830 break; 831 832 case CP_OPCODE_REQUEST_CHAIN_WEIGHT: 833 if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break; 834 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 835 break; 836 case CP_OPCODE_SET_CHAIN_WEIGHT: 837 if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break; 838 instance->chain_weight_g = little_endian_read_16(buffer, pos); 839 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 840 break; 841 842 case CP_OPCODE_REQUEST_SPAN_LENGTH: 843 if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 844 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 845 break; 846 case CP_OPCODE_SET_SPAN_LENGTH: 847 if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 848 instance->span_length_mm = little_endian_read_16(buffer, pos); 849 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 850 break; 851 852 case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE: 853 if (!has_feature(CP_FEATURE_FLAG_FACTORY_CALIBRATION_DATE_SUPPORTED)) break; 854 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 855 break; 856 857 case CP_OPCODE_REQUEST_SAMPLING_RATE: 858 if (!instance->vector_value_handle) break; 859 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 860 break; 861 862 case CP_OPCODE_START_OFFSET_COMPENSATION: 863 case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION: 864 if (!has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_SUPPORTED)){ 865 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 866 break; 867 } 868 if (has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && 869 ((has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE) || 870 (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE)) 871 ){ 872 instance->response_value = CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE; 873 cycling_power_sensor_measurement_context_t measurement_type = 874 has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) ? CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE : CP_SENSOR_MEASUREMENT_CONTEXT_FORCE; 875 cycling_power_service_server_emit_start_calibration(instance, measurement_type, instance->request_opcode == 876 CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION); 877 return 0; 878 } 879 instance->current_force_magnitude_N = 0xffff; 880 instance->current_torque_magnitude_Nm = 0xffff; 881 break; 882 883 case CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT:{ 884 if (!has_feature(CP_FEATURE_FLAG_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT_MASKING_SUPPORTED)) break; 885 uint16_t mask_bitmap = little_endian_read_16(buffer, pos); 886 uint16_t masked_measurement_flags = instance->default_measurement_flags; 887 uint16_t index = 0; 888 889 for (i = 0; i < CP_MASK_BIT_RESERVED; i++){ 890 uint8_t clear_bit = (mask_bitmap & (1u << i)) ? 1u : 0u; 891 892 masked_measurement_flags &= ~(clear_bit << index); 893 index++; 894 // following measurement flags have additional flag 895 switch ((cycling_power_mask_bit_t)i){ 896 case CP_MASK_BIT_PEDAL_POWER_BALANCE: 897 case CP_MASK_BIT_ACCUMULATED_TORQUE: 898 case CP_MASK_BIT_EXTREME_MAGNITUDES: 899 masked_measurement_flags &= ~(clear_bit << index); 900 index++; 901 break; 902 default: 903 break; 904 } 905 } 906 instance->masked_measurement_flags = masked_measurement_flags; 907 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 908 break; 909 } 910 default: 911 break; 912 } 913 914 if (instance->control_point_client_configuration_descriptor_indicate){ 915 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 916 instance->control_point_indicate_callback.context = (void*) instance; 917 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 918 } 919 return 0; 920 } 921 return 0; 922 } 923 924 static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 925 UNUSED(channel); 926 UNUSED(size); 927 cycling_power_t * instance = &cycling_power; 928 uint8_t event_type = hci_event_packet_get_type(packet); 929 uint16_t con_handle; 930 931 if (packet_type != HCI_EVENT_PACKET) return; 932 switch (event_type){ 933 case HCI_EVENT_META_GAP: 934 switch (hci_event_gap_meta_get_subevent_code(packet)) { 935 case GAP_SUBEVENT_LE_CONNECTION_COMPLETE: 936 instance->con_handle = gap_subevent_le_connection_complete_get_connection_handle(packet); 937 // print connection parameters (without using float operations) 938 instance->con_interval = gap_subevent_le_connection_complete_get_conn_interval(packet); 939 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_RECEIVED; 940 break; 941 default: 942 break; 943 } 944 break; 945 case HCI_EVENT_LE_META: 946 switch (hci_event_le_meta_get_subevent_code(packet)){ 947 #ifdef ENABLE_ATT_DELAYED_RESPONSE 948 case HCI_SUBEVENT_LE_CONNECTION_UPDATE_COMPLETE: 949 if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE) return; 950 951 if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){ 952 instance->con_interval = hci_subevent_le_connection_update_complete_get_conn_interval(packet); 953 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_ACCEPTED; 954 } else { 955 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED; 956 } 957 att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet)); 958 break; 959 #endif 960 default: 961 break; 962 } 963 break; 964 965 #ifdef ENABLE_ATT_DELAYED_RESPONSE 966 case L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE: 967 if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE) return; 968 969 if (l2cap_event_connection_parameter_update_response_get_result(packet) == ERROR_CODE_SUCCESS){ 970 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE; 971 } else { 972 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED; 973 att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet)); 974 } 975 break; 976 #endif 977 978 case HCI_EVENT_DISCONNECTION_COMPLETE:{ 979 if (!instance) return; 980 con_handle = hci_event_disconnection_complete_get_connection_handle(packet); 981 if (con_handle == HCI_CON_HANDLE_INVALID) return; 982 983 instance->masked_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 984 instance->w4_indication_complete = 0; 985 986 uint8_t event[5]; 987 int index = 0; 988 event[index++] = HCI_EVENT_GATTSERVICE_META; 989 event[index++] = sizeof(event) - 2u; 990 991 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP; 992 little_endian_store_16(event, index, con_handle); 993 index += 2; 994 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 995 996 break; 997 } 998 case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE: 999 instance->w4_indication_complete = 0; 1000 break; 1001 default: 1002 break; 1003 } 1004 } 1005 1006 void cycling_power_service_server_init(uint32_t feature_flags, 1007 cycling_power_pedal_power_balance_reference_t pedal_power_balance_reference, cycling_power_torque_source_t torque_source, 1008 cycling_power_sensor_location_t * supported_sensor_locations, uint16_t num_supported_sensor_locations, 1009 cycling_power_sensor_location_t current_sensor_location){ 1010 1011 cycling_power_t * instance = &cycling_power; 1012 // TODO: remove hardcoded initialization 1013 instance->con_interval_min = 6; 1014 instance->con_interval_max = 6; 1015 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_NONE; 1016 instance->w4_indication_complete = 0; 1017 1018 hci_event_callback_registration.callback = &packet_handler; 1019 hci_add_event_handler(&hci_event_callback_registration); 1020 1021 l2cap_event_callback_registration.callback = &packet_handler; 1022 l2cap_add_event_handler(&l2cap_event_callback_registration); 1023 1024 instance->sensor_location = current_sensor_location; 1025 instance->num_supported_sensor_locations = 0; 1026 if (supported_sensor_locations != NULL){ 1027 instance->num_supported_sensor_locations = num_supported_sensor_locations; 1028 instance->supported_sensor_locations = supported_sensor_locations; 1029 } 1030 1031 instance->feature_flags = feature_flags; 1032 instance->default_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 1033 instance->masked_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 1034 instance->pedal_power_balance_reference = pedal_power_balance_reference; 1035 instance->torque_source = torque_source; 1036 1037 // get service handle range 1038 uint16_t start_handle = 0; 1039 uint16_t end_handle = 0xffff; 1040 int service_found = gatt_server_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_POWER, &start_handle, &end_handle); 1041 btstack_assert(service_found != 0); 1042 UNUSED(service_found); 1043 1044 // get CP Mesurement characteristic value handle and client configuration handle 1045 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT); 1046 instance->measurement_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT); 1047 instance->measurement_server_configuration_descriptor_handle = gatt_server_get_server_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT); 1048 1049 // get CP Feature characteristic value handle and client configuration handle 1050 instance->feature_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_FEATURE); 1051 // get CP Sensor Location characteristic value handle and client configuration handle 1052 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 1053 1054 // get CP Vector characteristic value handle and client configuration handle 1055 instance->vector_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_VECTOR); 1056 instance->vector_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_VECTOR); 1057 1058 // get Body Sensor Location characteristic value handle and client configuration handle 1059 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 1060 1061 // get SP Control Point characteristic value handle and client configuration handle 1062 instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_CONTROL_POINT); 1063 instance->control_point_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_CONTROL_POINT); 1064 1065 log_info("Measurement value handle 0x%02x", instance->measurement_value_handle); 1066 log_info("M. Client Cfg value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle); 1067 log_info("M. Server Cfg value handle 0x%02x", instance->measurement_server_configuration_descriptor_handle); 1068 1069 log_info("Feature value handle 0x%02x", instance->feature_value_handle); 1070 log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle); 1071 1072 log_info("Vector value handle 0x%02x", instance->vector_value_handle); 1073 log_info("Vector Cfg. value handle 0x%02x", instance->vector_client_configuration_descriptor_handle); 1074 1075 log_info("Control Point value handle 0x%02x", instance->control_point_value_handle); 1076 log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle); 1077 1078 cycling_power_service.start_handle = start_handle; 1079 cycling_power_service.end_handle = end_handle; 1080 cycling_power_service.read_callback = &cycling_power_service_read_callback; 1081 cycling_power_service.write_callback = &cycling_power_service_write_callback; 1082 cycling_power_service.packet_handler = &packet_handler; 1083 att_server_register_service_handler(&cycling_power_service); 1084 } 1085 1086 1087 void cycling_power_service_server_add_torque(int16_t torque_Nm){ 1088 cycling_power_t * instance = &cycling_power; 1089 instance->accumulated_torque_Nm += torque_Nm; 1090 } 1091 1092 void cycling_power_service_server_add_wheel_revolution(int32_t wheel_revolution, uint16_t wheel_event_time_s){ 1093 cycling_power_t * instance = &cycling_power; 1094 instance->last_wheel_event_time_s = wheel_event_time_s; 1095 if (wheel_revolution < 0){ 1096 uint32_t wheel_revolution_to_subtract = (uint32_t) (-wheel_revolution); 1097 if (instance->cumulative_wheel_revolutions > wheel_revolution_to_subtract){ 1098 instance->cumulative_wheel_revolutions -= wheel_revolution_to_subtract; 1099 } else { 1100 instance->cumulative_wheel_revolutions = 0; 1101 } 1102 } else { 1103 if (instance->cumulative_wheel_revolutions < (0xffffffff - wheel_revolution)){ 1104 instance->cumulative_wheel_revolutions += wheel_revolution; 1105 } else { 1106 instance->cumulative_wheel_revolutions = 0xffffffff; 1107 } 1108 } 1109 } 1110 1111 void cycling_power_service_server_add_crank_revolution(uint16_t crank_revolution, uint16_t crank_event_time_s){ 1112 cycling_power_t * instance = &cycling_power; 1113 instance->last_crank_event_time_s = crank_event_time_s; 1114 instance->cumulative_crank_revolutions += crank_revolution; 1115 } 1116 1117 void cycling_power_service_add_energy(uint16_t energy_kJ){ 1118 cycling_power_t * instance = &cycling_power; 1119 if (instance->accumulated_energy_kJ <= (0xffffu - energy_kJ)){ 1120 instance->accumulated_energy_kJ += energy_kJ; 1121 } else { 1122 instance->accumulated_energy_kJ = 0xffff; 1123 } 1124 } 1125 1126 void cycling_power_service_server_set_instantaneous_power(int16_t instantaneous_power_W){ 1127 cycling_power_t * instance = &cycling_power; 1128 instance->instantaneous_power_W = instantaneous_power_W; 1129 } 1130 1131 void cycling_power_service_server_set_pedal_power_balance(uint8_t pedal_power_balance_percentage){ 1132 cycling_power_t * instance = &cycling_power; 1133 instance->pedal_power_balance_percentage = pedal_power_balance_percentage; 1134 } 1135 1136 void cycling_power_service_server_set_force_magnitude_values(int force_magnitude_count, int16_t * force_magnitude_N_array){ 1137 cycling_power_t * instance = &cycling_power; 1138 instance->force_magnitude_count = force_magnitude_count; 1139 instance->vector_instantaneous_force_magnitude_N_array = force_magnitude_N_array; 1140 } 1141 1142 void cycling_power_service_server_set_torque_magnitude_values(int torque_magnitude_count, int16_t * torque_magnitude_Nm_array){ 1143 cycling_power_t * instance = &cycling_power; 1144 instance->torque_magnitude_count = torque_magnitude_count; 1145 instance->vector_instantaneous_torque_magnitude_Nm_array = torque_magnitude_Nm_array; 1146 } 1147 1148 void cycling_power_service_server_set_first_crank_measurement_angle(uint16_t first_crank_measurement_angle_degree){ 1149 cycling_power_t * instance = &cycling_power; 1150 instance->vector_first_crank_measurement_angle_degree = first_crank_measurement_angle_degree; 1151 } 1152 1153 void cycling_power_service_server_set_instantaneous_measurement_direction(cycling_power_instantaneous_measurement_direction_t direction){ 1154 cycling_power_t * instance = &cycling_power; 1155 instance->vector_instantaneous_measurement_direction = direction; 1156 } 1157 1158 void cycling_power_service_server_set_force_magnitude(int16_t min_force_magnitude_N, int16_t max_force_magnitude_N){ 1159 cycling_power_t * instance = &cycling_power; 1160 instance->minimum_force_magnitude_N = min_force_magnitude_N; 1161 instance->maximum_force_magnitude_N = max_force_magnitude_N; 1162 } 1163 1164 void cycling_power_service_server_set_torque_magnitude(int16_t min_torque_magnitude_Nm, int16_t max_torque_magnitude_Nm){ 1165 cycling_power_t * instance = &cycling_power; 1166 instance->minimum_torque_magnitude_Nm = min_torque_magnitude_Nm; 1167 instance->maximum_torque_magnitude_Nm = max_torque_magnitude_Nm; 1168 } 1169 1170 void cycling_power_service_server_set_angle(uint16_t min_angle_degree, uint16_t max_angle_degree){ 1171 cycling_power_t * instance = &cycling_power; 1172 instance->minimum_angle_degree = min_angle_degree; 1173 instance->maximum_angle_degree = max_angle_degree; 1174 } 1175 1176 void cycling_power_service_server_set_top_dead_spot_angle(uint16_t top_dead_spot_angle_degree){ 1177 cycling_power_t * instance = &cycling_power; 1178 instance->top_dead_spot_angle_degree = top_dead_spot_angle_degree; 1179 } 1180 1181 void cycling_power_service_server_set_bottom_dead_spot_angle(uint16_t bottom_dead_spot_angle_degree){ 1182 cycling_power_t * instance = &cycling_power; 1183 instance->bottom_dead_spot_angle_degree = bottom_dead_spot_angle_degree; 1184 } 1185 1186 static int gatt_date_is_valid(gatt_date_time_t date){ 1187 if ((date.year != 0u) && ((date.year < 1582u) || (date.year > 9999u))) return 0u; 1188 if ((date.month != 0u) && (date.month > 12u)) return 0u; 1189 if ((date.day != 0u) && (date.day > 31u)) return 0u; 1190 1191 if (date.hours > 23u) return 0u; 1192 if (date.minutes > 59u) return 0u; 1193 if (date.seconds > 59u) return 0u; 1194 return 1; 1195 } 1196 1197 int cycling_power_service_server_set_factory_calibration_date(gatt_date_time_t date){ 1198 if (!gatt_date_is_valid(date)) return 0; 1199 1200 cycling_power_t * instance = &cycling_power; 1201 instance->factory_calibration_date = date; 1202 return 1; 1203 } 1204 1205 void cycling_power_service_server_set_sampling_rate(uint8_t sampling_rate_Hz){ 1206 cycling_power_t * instance = &cycling_power; 1207 instance->sampling_rate_Hz = sampling_rate_Hz; 1208 } 1209 1210 1211 void cycling_power_service_server_update_values(void){ 1212 cycling_power_t * instance = &cycling_power; 1213 1214 if (instance->measurement_server_configuration_descriptor_broadcast){ 1215 instance->measurement_broadcast_callback.callback = &cycling_power_service_broadcast_can_send_now; 1216 instance->measurement_broadcast_callback.context = (void*) instance; 1217 att_server_register_can_send_now_callback(&instance->measurement_broadcast_callback, instance->con_handle); 1218 } 1219 1220 if (instance->measurement_client_configuration_descriptor_notify){ 1221 instance->measurement_notify_callback.callback = &cycling_power_service_measurement_can_send_now; 1222 instance->measurement_notify_callback.context = (void*) instance; 1223 att_server_register_can_send_now_callback(&instance->measurement_notify_callback, instance->con_handle); 1224 } 1225 1226 if (instance->vector_client_configuration_descriptor_notify){ 1227 instance->vector_notify_callback.callback = &cycling_power_service_vector_can_send_now; 1228 instance->vector_notify_callback.context = (void*) instance; 1229 att_server_register_can_send_now_callback(&instance->vector_notify_callback, instance->con_handle); 1230 } 1231 } 1232 1233 void cycling_power_service_server_packet_handler(btstack_packet_handler_t callback){ 1234 if (callback == NULL){ 1235 log_error("cycling_power_service_server_packet_handler called with NULL callback"); 1236 return; 1237 } 1238 cycling_power_t * instance = &cycling_power; 1239 instance->calibration_callback = callback; 1240 } 1241 1242 void cycling_power_server_calibration_done(cycling_power_sensor_measurement_context_t measurement_type, uint16_t calibrated_value){ 1243 cycling_power_t * instance = &cycling_power; 1244 if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){ 1245 return; 1246 } 1247 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 1248 1249 switch (measurement_type){ 1250 case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE: 1251 instance->current_force_magnitude_N = calibrated_value; 1252 break; 1253 case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE: 1254 instance->current_torque_magnitude_Nm = calibrated_value; 1255 break; 1256 default: 1257 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 1258 break; 1259 } 1260 1261 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 1262 switch (calibrated_value){ 1263 case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION: 1264 case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS: 1265 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1266 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1267 break; 1268 default: 1269 break; 1270 } 1271 } 1272 1273 if (instance->control_point_client_configuration_descriptor_indicate){ 1274 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 1275 instance->control_point_indicate_callback.context = (void*) instance; 1276 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 1277 } 1278 } 1279 1280 void cycling_power_server_enhanced_calibration_done(cycling_power_sensor_measurement_context_t measurement_type, 1281 uint16_t calibrated_value, uint16_t manufacturer_company_id, 1282 uint8_t num_manufacturer_specific_data, uint8_t * manufacturer_specific_data){ 1283 cycling_power_t * instance = &cycling_power; 1284 if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE) return; 1285 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 1286 1287 switch (measurement_type){ 1288 case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE: 1289 instance->current_force_magnitude_N = calibrated_value; 1290 break; 1291 case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE: 1292 instance->current_torque_magnitude_Nm = calibrated_value; 1293 break; 1294 default: 1295 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 1296 break; 1297 } 1298 1299 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 1300 switch (calibrated_value){ 1301 case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION: 1302 case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS: 1303 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1304 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1305 break; 1306 default: 1307 break; 1308 } 1309 instance->manufacturer_company_id = manufacturer_company_id; 1310 instance->num_manufacturer_specific_data = num_manufacturer_specific_data; 1311 instance->manufacturer_specific_data = manufacturer_specific_data; 1312 } 1313 1314 if (instance->control_point_client_configuration_descriptor_indicate){ 1315 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 1316 instance->control_point_indicate_callback.context = (void*) instance; 1317 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 1318 } 1319 } 1320