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 MATTHIAS 24 * RINGWALD 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_watt; 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_m; // 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_newton; 139 int16_t minimum_force_magnitude_newton; 140 int16_t maximum_torque_magnitude_newton_m; // newton meters, resolution 1/32 141 int16_t minimum_torque_magnitude_newton_m; // newton meters, resolution 1/32 142 // extreme angles 143 uint16_t maximum_angle_deg; // 12bit, degrees 144 uint16_t minimum_angle_deg; // 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_deg; 147 uint16_t bottom_dead_spot_angle_deg; // 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_newton; 183 int16_t current_torque_magnitude_newton_m; // 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_deg; 193 int16_t * vector_instantaneous_force_magnitude_newton_array; // newton 194 int force_magnitude_count; 195 int16_t * vector_instantaneous_torque_magnitude_newton_per_m_array; // newton per 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 222 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){ 223 UNUSED(con_handle); 224 UNUSED(attribute_handle); 225 UNUSED(offset); 226 cycling_power_t * instance = &cycling_power; 227 228 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 229 if (buffer && (buffer_size >= 2u)){ 230 little_endian_store_16(buffer, 0, instance->measurement_client_configuration_descriptor_notify); 231 } 232 return 2; 233 } 234 235 if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){ 236 if (buffer && (buffer_size >= 2u)){ 237 little_endian_store_16(buffer, 0, instance->measurement_server_configuration_descriptor_broadcast); 238 } 239 return 2; 240 } 241 242 if (attribute_handle == instance->vector_client_configuration_descriptor_handle){ 243 if (buffer && (buffer_size >= 2u)){ 244 little_endian_store_16(buffer, 0, instance->vector_client_configuration_descriptor_notify); 245 } 246 return 2; 247 } 248 249 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 250 if (buffer && (buffer_size >= 2u)){ 251 little_endian_store_16(buffer, 0, instance->control_point_client_configuration_descriptor_indicate); 252 } 253 return 2; 254 } 255 256 if (attribute_handle == instance->feature_value_handle){ 257 if (buffer && (buffer_size >= 4u)){ 258 little_endian_store_32(buffer, 0, instance->feature_flags); 259 } 260 return 4; 261 } 262 263 if (attribute_handle == instance->sensor_location_value_handle){ 264 if (buffer && (buffer_size >= 1u)){ 265 buffer[0] = instance->sensor_location; 266 } 267 return 1; 268 } 269 return 0; 270 } 271 272 static int has_feature(cycling_power_feature_flag_t feature){ 273 cycling_power_t * instance = &cycling_power; 274 return (instance->feature_flags & (1u << feature)) != 0u; 275 } 276 277 static int cycling_power_vector_instantaneous_measurement_direction(void){ 278 cycling_power_t * instance = &cycling_power; 279 return instance->vector_instantaneous_measurement_direction; 280 } 281 282 static uint16_t cycling_power_service_default_measurement_flags(void){ 283 cycling_power_t * instance = &cycling_power; 284 uint16_t measurement_flags = 0; 285 uint8_t flag[] = { 286 (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED), 287 (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED) && instance->pedal_power_balance_reference, 288 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED), 289 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED) && instance->torque_source, 290 (uint8_t) has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED), 291 (uint8_t) has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED), 292 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE), 293 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE), 294 (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED), 295 (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED), 296 (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED), 297 (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_ENERGY_SUPPORTED), 298 (uint8_t) has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_INDICATOR_SUPPORTED) 299 }; 300 301 int i; 302 for (i = CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT; i <= CP_MEASUREMENT_FLAG_OFFSET_COMPENSATION_INDICATOR; i++){ 303 measurement_flags |= flag[i] << i; 304 } 305 306 return measurement_flags; 307 } 308 309 static uint16_t cycling_power_service_get_measurement_flags(cycling_power_t * instance){ 310 if (!instance) return 0; 311 if (instance->masked_measurement_flags != CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){ 312 return instance->masked_measurement_flags; 313 } 314 if (instance->default_measurement_flags == CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){ 315 instance->default_measurement_flags = cycling_power_service_default_measurement_flags(); 316 } 317 return instance->default_measurement_flags; 318 } 319 320 321 uint16_t cycling_power_service_measurement_flags(void){ 322 cycling_power_t * instance = &cycling_power; 323 return cycling_power_service_get_measurement_flags(instance); 324 } 325 326 uint8_t cycling_power_service_vector_flags(void){ 327 uint8_t vector_flags = 0; 328 uint8_t flag[] = { 329 (uint8_t )has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED), 330 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED), 331 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE), 332 (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE), 333 (uint8_t )has_feature(CP_FEATURE_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION_SUPPORTED) && cycling_power_vector_instantaneous_measurement_direction() 334 }; 335 336 int i; 337 for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){ 338 vector_flags |= flag[i] << i; 339 } 340 return vector_flags; 341 } 342 343 static void cycling_power_service_vector_can_send_now(void * context){ 344 cycling_power_t * instance = (cycling_power_t *) context; 345 if (!instance){ 346 log_error("cycling_power_service_measurement_can_send_now: instance is null"); 347 return; 348 } 349 uint8_t value[50]; 350 uint8_t vector_flags = cycling_power_service_vector_flags(); 351 int pos = 0; 352 353 value[pos++] = vector_flags; 354 int i; 355 for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){ 356 if ((vector_flags & (1u << i)) == 0u) continue; 357 switch ((cycling_power_vector_flag_t) i){ 358 case CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 359 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 360 pos += 2; 361 little_endian_store_16(value, pos, instance->last_crank_event_time_s); 362 pos += 2; 363 break; 364 case CP_VECTOR_FLAG_INSTANTANEOUS_FORCE_MAGNITUDE_ARRAY_PRESENT:{ 365 uint16_t att_mtu = att_server_get_mtu(instance->con_handle); 366 uint16_t bytes_left = 0; 367 if (att_mtu > (pos + 3u)){ 368 bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos); 369 } 370 while ((bytes_left > 2u) && instance->force_magnitude_count){ 371 little_endian_store_16(value, pos, instance->vector_instantaneous_force_magnitude_newton_array[0]); 372 pos += 2; 373 bytes_left -= 2u; 374 instance->vector_instantaneous_force_magnitude_newton_array++; 375 instance->force_magnitude_count--; 376 } 377 break; 378 } 379 case CP_VECTOR_FLAG_INSTANTANEOUS_TORQUE_MAGNITUDE_ARRAY_PRESENT:{ 380 uint16_t att_mtu = att_server_get_mtu(instance->con_handle); 381 uint16_t bytes_left = 0; 382 if (att_mtu > (pos + 3u)){ 383 bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos); 384 } 385 386 while ((bytes_left > 2u) && instance->torque_magnitude_count){ 387 little_endian_store_16(value, pos, instance->vector_instantaneous_torque_magnitude_newton_per_m_array[0]); 388 pos += 2; 389 bytes_left -= 2u; 390 instance->vector_instantaneous_torque_magnitude_newton_per_m_array++; 391 instance->torque_magnitude_count--; 392 } 393 break; 394 } 395 case CP_VECTOR_FLAG_FIRST_CRANK_MEASUREMENT_ANGLE_PRESENT: 396 little_endian_store_16(value, pos, instance->vector_first_crank_measurement_angle_deg); 397 pos += 2; 398 break; 399 case CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION: 400 break; 401 default: 402 break; 403 } 404 } 405 406 att_server_notify(instance->con_handle, instance->vector_value_handle, &value[0], pos); 407 } 408 409 static int cycling_power_measurement_flag_value_size(cycling_power_measurement_flag_t flag){ 410 switch (flag){ 411 case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT: 412 return 1; 413 case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT: 414 return 6; 415 case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 416 case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT: 417 case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT: 418 return 4; 419 case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT: 420 return 3; 421 case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT: 422 case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT: 423 case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT: 424 case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT: 425 return 2; 426 default: 427 return 0; 428 } 429 } 430 431 static int cycling_power_store_measurement_flag_value(cycling_power_t * instance, cycling_power_measurement_flag_t flag, uint8_t * value){ 432 if (!instance) return 0; 433 434 int pos = 0; 435 switch (flag){ 436 case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT: 437 value[pos++] = instance->pedal_power_balance_percentage; 438 break; 439 case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT: 440 little_endian_store_16(value, pos, instance->accumulated_torque_m); 441 pos += 2; 442 break; 443 case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT: 444 little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions); 445 pos += 4; 446 little_endian_store_16(value, pos, instance->last_wheel_event_time_s); 447 pos += 2; 448 break; 449 case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT: 450 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 451 pos += 2; 452 little_endian_store_16(value, pos, instance->last_crank_event_time_s); 453 pos += 2; 454 break; 455 case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT: 456 little_endian_store_16(value, pos, (uint16_t)instance->maximum_force_magnitude_newton); 457 pos += 2; 458 little_endian_store_16(value, pos, (uint16_t)instance->minimum_force_magnitude_newton); 459 pos += 2; 460 break; 461 case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT: 462 little_endian_store_16(value, pos, (uint16_t)instance->maximum_torque_magnitude_newton_m); 463 pos += 2; 464 little_endian_store_16(value, pos, (uint16_t)instance->minimum_torque_magnitude_newton_m); 465 pos += 2; 466 break; 467 case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT: 468 little_endian_store_24(value, pos, (instance->maximum_angle_deg << 12) | instance->minimum_angle_deg); 469 pos += 3; 470 break; 471 case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT: 472 little_endian_store_16(value, pos, (uint16_t)instance->top_dead_spot_angle_deg); 473 pos += 2; 474 break; 475 case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT: 476 little_endian_store_16(value, pos, (uint16_t)instance->bottom_dead_spot_angle_deg); 477 pos += 2; 478 break; 479 case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT: 480 little_endian_store_16(value, pos, (uint16_t)instance->accumulated_energy_kJ); 481 pos += 2; 482 break; 483 default: 484 break; 485 } 486 return pos; 487 } 488 489 490 static int cycling_power_store_measurement(cycling_power_t * instance, uint8_t * value, uint16_t max_value_size){ 491 if (max_value_size < 4u) return 0u; 492 if (!instance) return 0; 493 494 uint16_t measurement_flags = cycling_power_service_get_measurement_flags(instance); 495 int pos = 0; 496 little_endian_store_16(value, 0, measurement_flags); 497 pos += 2; 498 little_endian_store_16(value, 2, instance->instantaneous_power_watt); 499 pos += 2; 500 int flag_index; 501 uint16_t bytes_left = max_value_size - pos; 502 for (flag_index = 0; flag_index < CP_MEASUREMENT_FLAG_RESERVED; flag_index++){ 503 if ((measurement_flags & (1u << flag_index)) == 0u) continue; 504 cycling_power_measurement_flag_t flag = (cycling_power_measurement_flag_t) flag_index; 505 uint16_t value_size = cycling_power_measurement_flag_value_size(flag); 506 if (value_size > bytes_left ) return pos; 507 cycling_power_store_measurement_flag_value(instance, flag, &value[pos]); 508 pos += value_size; 509 bytes_left -= value_size; 510 } 511 return pos; 512 } 513 514 int cycling_power_get_measurement_adv(uint16_t adv_interval, uint8_t * broadcast_adv, uint16_t max_value_size){ 515 if (max_value_size < 12u) return 0u; 516 cycling_power_t * instance = &cycling_power; 517 int pos = 0; 518 // adv flags 519 broadcast_adv[pos++] = 2; 520 broadcast_adv[pos++] = BLUETOOTH_DATA_TYPE_FLAGS; 521 broadcast_adv[pos++] = 0x4; 522 523 // adv interval 524 broadcast_adv[pos++] = 3; 525 broadcast_adv[pos++] = BLUETOOTH_DATA_TYPE_ADVERTISING_INTERVAL; 526 little_endian_store_16(broadcast_adv, pos, adv_interval); 527 pos += 2; 528 // 529 int value_len = cycling_power_store_measurement(instance, &broadcast_adv[pos+4], CYCLING_POWER_MAX_BROACAST_MSG_SIZE - (pos + 4)); 530 broadcast_adv[pos++] = 3 + value_len; 531 broadcast_adv[pos++] = BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID; 532 little_endian_store_16(broadcast_adv, pos, ORG_BLUETOOTH_SERVICE_CYCLING_POWER); 533 pos += 2; 534 // value data already in place cycling_power_get_measurement 535 pos += value_len; 536 // set ADV_NONCONN_IND 537 return pos; 538 } 539 540 static void cycling_power_service_broadcast_can_send_now(void * context){ 541 cycling_power_t * instance = (cycling_power_t *) context; 542 if (!instance){ 543 log_error("cycling_power_service_broadcast_can_send_now: instance is null"); 544 return; 545 } 546 uint8_t value[CYCLING_POWER_MAX_BROACAST_MSG_SIZE]; 547 int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value)); 548 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 549 } 550 551 static void cycling_power_service_measurement_can_send_now(void * context){ 552 cycling_power_t * instance = (cycling_power_t *) context; 553 if (!instance){ 554 log_error("cycling_power_service_measurement_can_send_now: instance is null"); 555 return; 556 } 557 uint8_t value[40]; 558 int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value)); 559 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 560 } 561 562 static void cycling_power_service_response_can_send_now(void * context){ 563 cycling_power_t * instance = (cycling_power_t *) context; 564 if (!instance){ 565 log_error("cycling_power_service_response_can_send_now: instance is null"); 566 return; 567 } 568 569 if (instance->response_value == CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){ 570 log_error("cycling_power_service_response_can_send_now: CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE"); 571 return; 572 } 573 574 // use preprocessor instead of btstack_max to get compile-time constant 575 #if (CP_SENSOR_LOCATION_RESERVED > (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5)) 576 #define MAX_RESPONSE_PAYLOAD CP_SENSOR_LOCATION_RESERVED 577 #else 578 #define MAX_RESPONSE_PAYLOAD (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5) 579 #endif 580 581 uint8_t value[3 + MAX_RESPONSE_PAYLOAD]; 582 int pos = 0; 583 value[pos++] = CP_OPCODE_RESPONSE_CODE; 584 value[pos++] = instance->request_opcode; 585 value[pos++] = instance->response_value; 586 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 587 switch (instance->request_opcode){ 588 case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{ 589 int i; 590 for (i=0; i<instance->num_supported_sensor_locations; i++){ 591 value[pos++] = instance->supported_sensor_locations[i]; 592 } 593 break; 594 } 595 case CP_OPCODE_REQUEST_CRANK_LENGTH: 596 little_endian_store_16(value, pos, instance->crank_length_mm); 597 pos += 2; 598 break; 599 case CP_OPCODE_REQUEST_CHAIN_LENGTH: 600 little_endian_store_16(value, pos, instance->chain_length_mm); 601 pos += 2; 602 break; 603 case CP_OPCODE_REQUEST_CHAIN_WEIGHT: 604 little_endian_store_16(value, pos, instance->chain_weight_g); 605 pos += 2; 606 break; 607 case CP_OPCODE_REQUEST_SPAN_LENGTH: 608 little_endian_store_16(value, pos, instance->span_length_mm); 609 pos += 2; 610 break; 611 case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE: 612 little_endian_store_16(value, pos, instance->factory_calibration_date.year); 613 pos += 2; 614 value[pos++] = instance->factory_calibration_date.month; 615 value[pos++] = instance->factory_calibration_date.day; 616 value[pos++] = instance->factory_calibration_date.hours; 617 value[pos++] = instance->factory_calibration_date.minutes; 618 value[pos++] = instance->factory_calibration_date.seconds; 619 break; 620 case CP_OPCODE_REQUEST_SAMPLING_RATE: 621 value[pos++] = instance->sampling_rate_hz; 622 break; 623 case CP_OPCODE_START_OFFSET_COMPENSATION: 624 case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION:{ 625 uint16_t calibrated_value = 0xffff; 626 if (has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED)){ 627 if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE) { 628 calibrated_value = instance->current_force_magnitude_newton; 629 } else if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE){ 630 calibrated_value = instance->current_torque_magnitude_newton_m; 631 } 632 } 633 634 if (calibrated_value == CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION){ 635 value[pos++] = calibrated_value; 636 // do not include manufacturer ID and data 637 break; 638 } else if (calibrated_value == CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS){ 639 value[pos++] = calibrated_value; 640 } else { 641 little_endian_store_16(value, pos, calibrated_value); 642 pos += 2; 643 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 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){ 673 UNUSED(con_handle); 674 UNUSED(transaction_mode); 675 UNUSED(offset); 676 UNUSED(buffer_size); 677 int i; 678 cycling_power_sensor_location_t location; 679 cycling_power_t * instance = &cycling_power; 680 681 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 682 if (buffer_size < 2u){ 683 return ATT_ERROR_INVALID_OFFSET; 684 } 685 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 686 instance->con_handle = con_handle; 687 log_info("cycling_power_service_write_callback: measurement enabled %d", instance->measurement_client_configuration_descriptor_notify); 688 return 0; 689 } 690 691 if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){ 692 if (buffer_size < 2u){ 693 return ATT_ERROR_INVALID_OFFSET; 694 } 695 instance->measurement_server_configuration_descriptor_broadcast = little_endian_read_16(buffer, 0); 696 instance->con_handle = con_handle; 697 uint8_t event[5]; 698 int index = 0; 699 event[index++] = HCI_EVENT_GATTSERVICE_META; 700 event[index++] = sizeof(event) - 2u; 701 702 if (instance->measurement_server_configuration_descriptor_broadcast){ 703 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_START; 704 log_info("cycling_power_service_write_callback: start broadcast"); 705 } else { 706 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP; 707 log_info("cycling_power_service_write_callback: stop broadcast"); 708 } 709 little_endian_store_16(event, index, con_handle); 710 index += 2; 711 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 712 return 0; 713 } 714 715 if (attribute_handle == instance->vector_client_configuration_descriptor_handle){ 716 if (buffer_size < 2u){ 717 return ATT_ERROR_INVALID_OFFSET; 718 } 719 instance->con_handle = con_handle; 720 721 #ifdef ENABLE_ATT_DELAYED_RESPONSE 722 switch (instance->con_interval_status){ 723 case CP_CONNECTION_INTERVAL_STATUS_REJECTED: 724 return CYCLING_POWER_ERROR_CODE_INAPPROPRIATE_CONNECTION_PARAMETERS; 725 726 case CP_CONNECTION_INTERVAL_STATUS_ACCEPTED: 727 case CP_CONNECTION_INTERVAL_STATUS_RECEIVED: 728 if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){ 729 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE; 730 gap_request_connection_parameter_update(instance->con_handle, instance->con_interval_min, instance->con_interval_max, 4, 100); // 15 ms, 4, 1s 731 return ATT_ERROR_WRITE_RESPONSE_PENDING; 732 } 733 instance->vector_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 734 return 0; 735 default: 736 return ATT_ERROR_WRITE_RESPONSE_PENDING; 737 738 } 739 #endif 740 } 741 742 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 743 if (buffer_size < 2u){ 744 return ATT_ERROR_INVALID_OFFSET; 745 } 746 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 747 instance->con_handle = con_handle; 748 log_info("cycling_power_service_write_callback: indication enabled %d", instance->control_point_client_configuration_descriptor_indicate); 749 return 0; 750 } 751 752 if (attribute_handle == instance->feature_value_handle){ 753 if (buffer_size < 4u){ 754 return ATT_ERROR_INVALID_OFFSET; 755 } 756 instance->feature_flags = little_endian_read_32(buffer, 0); 757 return 0; 758 } 759 760 if (attribute_handle == instance->control_point_value_handle){ 761 if (instance->control_point_client_configuration_descriptor_indicate == 0u) return CYCLING_POWER_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 762 if (instance->w4_indication_complete != 0u){ 763 return CYCLING_POWER_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 764 } 765 int pos = 0; 766 instance->request_opcode = (cycling_power_opcode_t) buffer[pos++]; 767 instance->response_value = CP_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 768 769 switch (instance->request_opcode){ 770 case CP_OPCODE_SET_CUMULATIVE_VALUE: 771 if (!has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED)) break; 772 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, pos); 773 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 774 break; 775 776 case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 777 if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break; 778 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 779 break; 780 781 case CP_OPCODE_UPDATE_SENSOR_LOCATION: 782 if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break; 783 location = (cycling_power_sensor_location_t) buffer[pos]; 784 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 785 for (i=0; i<instance->num_supported_sensor_locations; i++){ 786 if (instance->supported_sensor_locations[i] == location){ 787 instance->sensor_location = location; 788 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 789 break; 790 } 791 } 792 break; 793 794 case CP_OPCODE_REQUEST_CRANK_LENGTH: 795 if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break; 796 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 797 break; 798 case CP_OPCODE_SET_CRANK_LENGTH: 799 if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break; 800 instance->crank_length_mm = little_endian_read_16(buffer, pos); 801 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 802 break; 803 804 case CP_OPCODE_REQUEST_CHAIN_LENGTH: 805 if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 806 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 807 break; 808 case CP_OPCODE_SET_CHAIN_LENGTH: 809 if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 810 instance->chain_length_mm = little_endian_read_16(buffer, pos); 811 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 812 break; 813 814 case CP_OPCODE_REQUEST_CHAIN_WEIGHT: 815 if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break; 816 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 817 break; 818 case CP_OPCODE_SET_CHAIN_WEIGHT: 819 if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break; 820 instance->chain_weight_g = little_endian_read_16(buffer, pos); 821 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 822 break; 823 824 case CP_OPCODE_REQUEST_SPAN_LENGTH: 825 if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 826 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 827 break; 828 case CP_OPCODE_SET_SPAN_LENGTH: 829 if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break; 830 instance->span_length_mm = little_endian_read_16(buffer, pos); 831 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 832 break; 833 834 case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE: 835 if (!has_feature(CP_FEATURE_FLAG_FACTORY_CALIBRATION_DATE_SUPPORTED)) break; 836 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 837 break; 838 839 case CP_OPCODE_REQUEST_SAMPLING_RATE: 840 if (!instance->vector_value_handle) break; 841 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 842 break; 843 844 case CP_OPCODE_START_OFFSET_COMPENSATION: 845 case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION: 846 if (!has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_SUPPORTED)){ 847 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 848 break; 849 } 850 if (has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && 851 ((has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE) || 852 (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE)) 853 ){ 854 uint8_t event[7]; 855 int index = 0; 856 event[index++] = HCI_EVENT_GATTSERVICE_META; 857 event[index++] = sizeof(event) - 2u; 858 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_START_CALIBRATION; 859 little_endian_store_16(event, index, con_handle); 860 index += 2; 861 event[index++] = has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE; 862 event[index++] = (instance->request_opcode == CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION); 863 instance->response_value = CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE; 864 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 865 return 0; 866 } 867 instance->current_force_magnitude_newton = 0xffff; 868 instance->current_torque_magnitude_newton_m = 0xffff; 869 break; 870 871 case CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT:{ 872 if (!has_feature(CP_FEATURE_FLAG_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT_MASKING_SUPPORTED)) break; 873 uint16_t mask_bitmap = little_endian_read_16(buffer, pos); 874 uint16_t masked_measurement_flags = instance->default_measurement_flags; 875 uint16_t index = 0; 876 877 for (i = 0; i < CP_MASK_BIT_RESERVED; i++){ 878 uint8_t clear_bit = (mask_bitmap & (1u << i)) ? 1u : 0u; 879 880 masked_measurement_flags &= ~(clear_bit << index); 881 index++; 882 // following measurement flags have additional flag 883 switch ((cycling_power_mask_bit_t)i){ 884 case CP_MASK_BIT_PEDAL_POWER_BALANCE: 885 case CP_MASK_BIT_ACCUMULATED_TORQUE: 886 case CP_MASK_BIT_EXTREME_MAGNITUDES: 887 masked_measurement_flags &= ~(clear_bit << index); 888 index++; 889 break; 890 default: 891 break; 892 } 893 } 894 instance->masked_measurement_flags = masked_measurement_flags; 895 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 896 break; 897 } 898 default: 899 break; 900 } 901 902 if (instance->control_point_client_configuration_descriptor_indicate){ 903 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 904 instance->control_point_indicate_callback.context = (void*) instance; 905 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 906 } 907 return 0; 908 } 909 return 0; 910 } 911 912 static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 913 UNUSED(channel); 914 UNUSED(size); 915 cycling_power_t * instance = &cycling_power; 916 uint8_t event_type = hci_event_packet_get_type(packet); 917 uint16_t con_handle; 918 919 if (packet_type != HCI_EVENT_PACKET) return; 920 switch (event_type){ 921 case HCI_EVENT_LE_META: 922 switch (hci_event_le_meta_get_subevent_code(packet)){ 923 case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: 924 instance->con_handle = hci_subevent_le_connection_complete_get_connection_handle(packet); 925 // print connection parameters (without using float operations) 926 instance->con_interval = hci_subevent_le_connection_complete_get_conn_interval(packet); 927 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_RECEIVED; 928 break; 929 930 #ifdef ENABLE_ATT_DELAYED_RESPONSE 931 case HCI_SUBEVENT_LE_CONNECTION_UPDATE_COMPLETE: 932 if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE) return; 933 934 if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){ 935 instance->con_interval = hci_subevent_le_connection_update_complete_get_conn_interval(packet); 936 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_ACCEPTED; 937 } else { 938 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED; 939 } 940 att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet)); 941 break; 942 #endif 943 default: 944 break; 945 } 946 break; 947 948 #ifdef ENABLE_ATT_DELAYED_RESPONSE 949 case L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE: 950 if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE) return; 951 952 if (l2cap_event_connection_parameter_update_response_get_result(packet) == ERROR_CODE_SUCCESS){ 953 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE; 954 } else { 955 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED; 956 att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet)); 957 } 958 break; 959 #endif 960 961 case HCI_EVENT_DISCONNECTION_COMPLETE:{ 962 if (!instance) return; 963 con_handle = hci_event_disconnection_complete_get_connection_handle(packet); 964 if (con_handle == HCI_CON_HANDLE_INVALID) return; 965 966 instance->masked_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 967 instance->w4_indication_complete = 0; 968 969 uint8_t event[5]; 970 int index = 0; 971 event[index++] = HCI_EVENT_GATTSERVICE_META; 972 event[index++] = sizeof(event) - 2u; 973 974 event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP; 975 little_endian_store_16(event, index, con_handle); 976 index += 2; 977 (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 978 979 break; 980 } 981 case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE: 982 instance->w4_indication_complete = 0; 983 break; 984 default: 985 break; 986 } 987 } 988 989 void cycling_power_service_server_init(uint32_t feature_flags, 990 cycling_power_pedal_power_balance_reference_t reference, cycling_power_torque_source_t torque_source, 991 cycling_power_sensor_location_t * supported_sensor_locations, uint16_t num_supported_sensor_locations, 992 cycling_power_sensor_location_t current_sensor_location){ 993 994 cycling_power_t * instance = &cycling_power; 995 // TODO: remove hardcoded initialization 996 instance->con_interval_min = 6; 997 instance->con_interval_max = 6; 998 instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_NONE; 999 instance->w4_indication_complete = 0; 1000 hci_event_callback_registration.callback = &packet_handler; 1001 hci_add_event_handler(&hci_event_callback_registration); 1002 l2cap_register_packet_handler(&packet_handler); 1003 1004 instance->sensor_location = current_sensor_location; 1005 instance->num_supported_sensor_locations = 0; 1006 if (supported_sensor_locations != NULL){ 1007 instance->num_supported_sensor_locations = num_supported_sensor_locations; 1008 instance->supported_sensor_locations = supported_sensor_locations; 1009 } 1010 1011 instance->feature_flags = feature_flags; 1012 instance->default_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 1013 instance->masked_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED; 1014 instance->pedal_power_balance_reference = reference; 1015 instance->torque_source = torque_source; 1016 1017 // get service handle range 1018 uint16_t start_handle = 0; 1019 uint16_t end_handle = 0xffff; 1020 int service_found = gatt_server_get_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_POWER, &start_handle, &end_handle); 1021 btstack_assert(service_found != 0); 1022 UNUSED(service_found); 1023 1024 // get CP Mesurement characteristic value handle and client configuration handle 1025 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT); 1026 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); 1027 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); 1028 1029 // get CP Feature characteristic value handle and client configuration handle 1030 instance->feature_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_FEATURE); 1031 // get CP Sensor Location characteristic value handle and client configuration handle 1032 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 1033 1034 // get CP Vector characteristic value handle and client configuration handle 1035 instance->vector_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_VECTOR); 1036 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); 1037 1038 // get Body Sensor Location characteristic value handle and client configuration handle 1039 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 1040 1041 // get SP Control Point characteristic value handle and client configuration handle 1042 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); 1043 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); 1044 1045 log_info("Measurement value handle 0x%02x", instance->measurement_value_handle); 1046 log_info("M. Client Cfg value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle); 1047 log_info("M. Server Cfg value handle 0x%02x", instance->measurement_server_configuration_descriptor_handle); 1048 1049 log_info("Feature value handle 0x%02x", instance->feature_value_handle); 1050 log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle); 1051 1052 log_info("Vector value handle 0x%02x", instance->vector_value_handle); 1053 log_info("Vector Cfg. value handle 0x%02x", instance->vector_client_configuration_descriptor_handle); 1054 1055 log_info("Control Point value handle 0x%02x", instance->control_point_value_handle); 1056 log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle); 1057 1058 cycling_power_service.start_handle = start_handle; 1059 cycling_power_service.end_handle = end_handle; 1060 cycling_power_service.read_callback = &cycling_power_service_read_callback; 1061 cycling_power_service.write_callback = &cycling_power_service_write_callback; 1062 cycling_power_service.packet_handler = &packet_handler; 1063 att_server_register_service_handler(&cycling_power_service); 1064 } 1065 1066 1067 void cycling_power_service_server_add_torque(int16_t torque_m){ 1068 cycling_power_t * instance = &cycling_power; 1069 instance->accumulated_torque_m += torque_m; 1070 } 1071 1072 void cycling_power_service_server_add_wheel_revolution(int32_t wheel_revolution, uint16_t wheel_event_time_s){ 1073 cycling_power_t * instance = &cycling_power; 1074 instance->last_wheel_event_time_s = wheel_event_time_s; 1075 if (wheel_revolution < 0){ 1076 if (instance->cumulative_wheel_revolutions > -wheel_revolution){ 1077 instance->cumulative_wheel_revolutions += wheel_revolution; 1078 } else { 1079 instance->cumulative_wheel_revolutions = 0; 1080 } 1081 } else { 1082 if (instance->cumulative_wheel_revolutions < (0xffffffff - wheel_revolution)){ 1083 instance->cumulative_wheel_revolutions += wheel_revolution; 1084 } else { 1085 instance->cumulative_wheel_revolutions = 0xffffffff; 1086 } 1087 } 1088 } 1089 1090 void cycling_power_service_server_add_crank_revolution(uint16_t crank_revolution, uint16_t crank_event_time_s){ 1091 cycling_power_t * instance = &cycling_power; 1092 instance->last_crank_event_time_s = crank_event_time_s; 1093 instance->cumulative_crank_revolutions += crank_revolution; 1094 } 1095 1096 void cycling_power_service_add_energy(uint16_t energy_kJ){ 1097 cycling_power_t * instance = &cycling_power; 1098 if (instance->accumulated_energy_kJ <= (0xffffu - energy_kJ)){ 1099 instance->accumulated_energy_kJ += energy_kJ; 1100 } else { 1101 instance->accumulated_energy_kJ = 0xffff; 1102 } 1103 } 1104 1105 void cycling_power_service_server_set_instantaneous_power(int16_t instantaneous_power_watt){ 1106 cycling_power_t * instance = &cycling_power; 1107 instance->instantaneous_power_watt = instantaneous_power_watt; 1108 } 1109 1110 void cycling_power_service_server_set_pedal_power_balance(uint8_t pedal_power_balance_percentage){ 1111 cycling_power_t * instance = &cycling_power; 1112 instance->pedal_power_balance_percentage = pedal_power_balance_percentage; 1113 } 1114 1115 void cycling_power_service_server_set_force_magnitude_values(int force_magnitude_count, int16_t * force_magnitude_newton_array){ 1116 cycling_power_t * instance = &cycling_power; 1117 instance->force_magnitude_count = force_magnitude_count; 1118 instance->vector_instantaneous_force_magnitude_newton_array = force_magnitude_newton_array; 1119 } 1120 1121 void cycling_power_service_server_set_torque_magnitude_values(int torque_magnitude_count, int16_t * torque_magnitude_newton_array){ 1122 cycling_power_t * instance = &cycling_power; 1123 instance->torque_magnitude_count = torque_magnitude_count; 1124 instance->vector_instantaneous_torque_magnitude_newton_per_m_array = torque_magnitude_newton_array; 1125 } 1126 1127 void cycling_power_service_server_set_first_crank_measurement_angle(uint16_t first_crank_measurement_angle_deg){ 1128 cycling_power_t * instance = &cycling_power; 1129 instance->vector_first_crank_measurement_angle_deg = first_crank_measurement_angle_deg; 1130 } 1131 1132 void cycling_power_service_server_set_instantaneous_measurement_direction(cycling_power_instantaneous_measurement_direction_t direction){ 1133 cycling_power_t * instance = &cycling_power; 1134 instance->vector_instantaneous_measurement_direction = direction; 1135 } 1136 1137 void cycling_power_service_server_set_force_magnitude(int16_t min_force_magnitude_newton, int16_t max_force_magnitude_newton){ 1138 cycling_power_t * instance = &cycling_power; 1139 instance->minimum_force_magnitude_newton = min_force_magnitude_newton; 1140 instance->maximum_force_magnitude_newton = max_force_magnitude_newton; 1141 } 1142 1143 void cycling_power_service_server_set_torque_magnitude(int16_t min_torque_magnitude_newton, int16_t max_torque_magnitude_newton){ 1144 cycling_power_t * instance = &cycling_power; 1145 instance->minimum_torque_magnitude_newton_m = min_torque_magnitude_newton; 1146 instance->maximum_torque_magnitude_newton_m = max_torque_magnitude_newton; 1147 } 1148 1149 void cycling_power_service_server_set_angle(uint16_t min_angle_deg, uint16_t max_angle_deg){ 1150 cycling_power_t * instance = &cycling_power; 1151 instance->minimum_angle_deg = min_angle_deg; 1152 instance->maximum_angle_deg = max_angle_deg; 1153 } 1154 1155 void cycling_power_service_server_set_top_dead_spot_angle(uint16_t top_dead_spot_angle_deg){ 1156 cycling_power_t * instance = &cycling_power; 1157 instance->top_dead_spot_angle_deg = top_dead_spot_angle_deg; 1158 } 1159 1160 void cycling_power_service_server_set_bottom_dead_spot_angle(uint16_t bottom_dead_spot_angle_deg){ 1161 cycling_power_t * instance = &cycling_power; 1162 instance->bottom_dead_spot_angle_deg = bottom_dead_spot_angle_deg; 1163 } 1164 1165 static int gatt_date_is_valid(gatt_date_time_t date){ 1166 if ((date.year != 0u) && ((date.year < 1582u) || (date.year > 9999u))) return 0u; 1167 if ((date.month != 0u) && (date.month > 12u)) return 0u; 1168 if ((date.day != 0u) && (date.day > 31u)) return 0u; 1169 1170 if (date.hours > 23u) return 0u; 1171 if (date.minutes > 59u) return 0u; 1172 if (date.seconds > 59u) return 0u; 1173 return 1; 1174 } 1175 1176 int cycling_power_service_server_set_factory_calibration_date(gatt_date_time_t date){ 1177 if (!gatt_date_is_valid(date)) return 0; 1178 1179 cycling_power_t * instance = &cycling_power; 1180 instance->factory_calibration_date = date; 1181 return 1; 1182 } 1183 1184 void cycling_power_service_server_set_sampling_rate(uint8_t sampling_rate_hz){ 1185 cycling_power_t * instance = &cycling_power; 1186 instance->sampling_rate_hz = sampling_rate_hz; 1187 } 1188 1189 1190 void cycling_power_service_server_update_values(void){ 1191 cycling_power_t * instance = &cycling_power; 1192 1193 if (instance->measurement_server_configuration_descriptor_broadcast){ 1194 instance->measurement_broadcast_callback.callback = &cycling_power_service_broadcast_can_send_now; 1195 instance->measurement_broadcast_callback.context = (void*) instance; 1196 att_server_register_can_send_now_callback(&instance->measurement_broadcast_callback, instance->con_handle); 1197 } 1198 1199 if (instance->measurement_client_configuration_descriptor_notify){ 1200 instance->measurement_notify_callback.callback = &cycling_power_service_measurement_can_send_now; 1201 instance->measurement_notify_callback.context = (void*) instance; 1202 att_server_register_can_send_now_callback(&instance->measurement_notify_callback, instance->con_handle); 1203 } 1204 1205 if (instance->vector_client_configuration_descriptor_notify){ 1206 instance->vector_notify_callback.callback = &cycling_power_service_vector_can_send_now; 1207 instance->vector_notify_callback.context = (void*) instance; 1208 att_server_register_can_send_now_callback(&instance->vector_notify_callback, instance->con_handle); 1209 } 1210 } 1211 1212 void cycling_power_service_server_packet_handler(btstack_packet_handler_t callback){ 1213 if (callback == NULL){ 1214 log_error("cycling_power_service_server_packet_handler called with NULL callback"); 1215 return; 1216 } 1217 cycling_power_t * instance = &cycling_power; 1218 instance->calibration_callback = callback; 1219 } 1220 1221 void cycling_power_server_calibration_done(cycling_power_sensor_measurement_context_t measurement_type, uint16_t calibrated_value){ 1222 cycling_power_t * instance = &cycling_power; 1223 if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){ 1224 return; 1225 } 1226 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 1227 1228 switch (measurement_type){ 1229 case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE: 1230 instance->current_force_magnitude_newton = calibrated_value; 1231 break; 1232 case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE: 1233 instance->current_torque_magnitude_newton_m = calibrated_value; 1234 break; 1235 default: 1236 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 1237 break; 1238 } 1239 1240 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 1241 switch (calibrated_value){ 1242 case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION: 1243 case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS: 1244 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1245 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1246 break; 1247 default: 1248 break; 1249 } 1250 } 1251 1252 if (instance->control_point_client_configuration_descriptor_indicate){ 1253 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 1254 instance->control_point_indicate_callback.context = (void*) instance; 1255 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 1256 } 1257 } 1258 1259 void cycling_power_server_enhanced_calibration_done(cycling_power_sensor_measurement_context_t measurement_type, 1260 uint16_t calibrated_value, uint16_t manufacturer_company_id, 1261 uint8_t num_manufacturer_specific_data, uint8_t * manufacturer_specific_data){ 1262 cycling_power_t * instance = &cycling_power; 1263 if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE) return; 1264 instance->response_value = CP_RESPONSE_VALUE_SUCCESS; 1265 1266 switch (measurement_type){ 1267 case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE: 1268 instance->current_force_magnitude_newton = calibrated_value; 1269 break; 1270 case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE: 1271 instance->current_torque_magnitude_newton_m = calibrated_value; 1272 break; 1273 default: 1274 instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER; 1275 break; 1276 } 1277 1278 if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){ 1279 switch (calibrated_value){ 1280 case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION: 1281 case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS: 1282 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1283 instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED; 1284 break; 1285 default: 1286 break; 1287 } 1288 instance->manufacturer_company_id = manufacturer_company_id; 1289 instance->num_manufacturer_specific_data = num_manufacturer_specific_data; 1290 instance->manufacturer_specific_data = manufacturer_specific_data; 1291 } 1292 1293 if (instance->control_point_client_configuration_descriptor_indicate){ 1294 instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now; 1295 instance->control_point_indicate_callback.context = (void*) instance; 1296 att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle); 1297 } 1298 } 1299