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