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