?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge pull request #11483 from SteveCEvans/elrs_race
[betaflight.git] / src / main / target / COLIBRI_RACE / i2c_bst.c
bloba5ef9e70a24efb2ddb1bdc648db723277e158083
1 /* By Larry Ho Ka Wai @ 23/06/2015*/
2
3 #include <stdbool.h>
4 #include <stdint.h>
5 #include <stdlib.h>
6 #include <string.h>
7 #include <math.h>
8
9 #include "build/build_config.h"
10 #include "build/debug.h"
11 #include "build/version.h"
12
13 #include "platform.h"
14
15 #include "common/axis.h"
16 #include "common/color.h"
17 #include "common/maths.h"
18
19 #include "config/config.h"
20 #include "config/config_eeprom.h"
21 #include "config/feature.h"
22
23 #include "drivers/accgyro/accgyro.h"
24 #include "drivers/compass/compass.h"
25 #include "drivers/bus_i2c.h"
26 #include "drivers/sensor.h"
27 #include "drivers/serial.h"
28 #include "drivers/system.h"
29 #include "drivers/time.h"
30 #include "drivers/timer.h"
31
32 #include "fc/controlrate_profile.h"
33 #include "fc/core.h"
34 #include "fc/rc_adjustments.h"
35 #include "fc/rc_controls.h"
36 #include "fc/runtime_config.h"
37
38 #include "flight/position.h"
39 #include "flight/failsafe.h"
40 #include "flight/imu.h"
41 #include "flight/mixer.h"
42 #include "flight/pid.h"
43 #include "flight/pid_init.h"
44 #include "flight/servos.h"
45
46 #include "io/servos.h"
47 #include "io/gps.h"
48 #include "io/gimbal.h"
49 #include "io/serial.h"
50 #include "io/ledstrip.h"
51 #include "io/flashfs.h"
52 #include "io/beeper.h"
53
54 #include "pg/motor.h"
55 #include "pg/rx.h"
56
57 #include "rx/rx.h"
58 #include "rx/msp.h"
59
60 #include "scheduler/scheduler.h"
61
62 #include "sensors/boardalignment.h"
63 #include "sensors/sensors.h"
64 #include "sensors/battery.h"
65 #include "sensors/acceleration.h"
66 #include "sensors/barometer.h"
67 #include "sensors/compass.h"
68 #include "sensors/gyro.h"
69 #include "sensors/rangefinder.h"
70
71 #include "telemetry/telemetry.h"
72
73 #include "bus_bst.h"
74 #include "i2c_bst.h"
75
76 #define GPS_POSITION_FRAME_ID 0x02
77 #define GPS_TIME_FRAME_ID 0x03
78 #define FC_ATTITUDE_FRAME_ID 0x1E
79 #define RC_CHANNEL_FRAME_ID 0x15
80 #define CROSSFIRE_RSSI_FRAME_ID 0x14
81 #define CLEANFLIGHT_MODE_FRAME_ID 0x20
82
83 #define BST_PROTOCOL_VERSION 0
84
85 #define API_VERSION_MAJOR 1 // increment when major changes are made
86 #define API_VERSION_MINOR 13 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
87
88 #define API_VERSION_LENGTH 2
89
90 //
91 // MSP commands for Cleanflight original features
92 //
93
94 #define BST_API_VERSION 1 //out message
95 #define BST_FC_VARIANT 2 //out message
96 #define BST_FC_VERSION 3 //out message
97 #define BST_BOARD_INFO 4 //out message
98 #define BST_BUILD_INFO 5 //out message
99
100 #define BST_MODE_RANGES 34 //out message Returns all mode ranges
101 #define BST_SET_MODE_RANGE 35 //in message Sets a single mode range
102 #define BST_FEATURE 36
103 #define BST_SET_FEATURE 37
104 #define BST_RX_CONFIG 44
105 #define BST_SET_RX_CONFIG 45
106 #define BST_LED_COLORS 46
107 #define BST_SET_LED_COLORS 47
108 #define BST_LED_STRIP_CONFIG 48
109 #define BST_SET_LED_STRIP_CONFIG 49
110 #define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter
111 #define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter
112 #define BST_RX_MAP 64 //out message Get channel map (also returns number of channels total)
113 #define BST_SET_RX_MAP 65 //in message Set rx map, numchannels to set comes from BST_RX_MAP
114 #define BST_REBOOT 68 //in message Reboot
115 #define BST_DISARM 70 //in message Disarm
116 #define BST_ENABLE_ARM 71 //in message Enable arm
117 #define BST_DEADBAND 72 //out message
118 #define BST_SET_DEADBAND 73 //in message
119 #define BST_FC_FILTERS 74 //out message
120 #define BST_SET_FC_FILTERS 75 //in message
121 #define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
122 #define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
123 #define BST_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
124 #define BST_PID 112 //out message P I D coeff (9 are used currently)
125 #define BST_MISC 114 //out message powermeter trig
126 #define BST_SET_PID 202 //in message P I D coeff (9 are used currently)
127 #define BST_ACC_CALIBRATION 205 //in message no param
128 #define BST_MAG_CALIBRATION 206 //in message no param
129 #define BST_SET_MISC 207 //in message powermeter trig + 8 free for future use
130 #define BST_SELECT_SETTING 210 //in message Select Setting Number (0-2)
131 #define BST_EEPROM_WRITE 250 //in message no param
132
133 extern volatile uint8_t CRC8;
134 extern volatile bool coreProReady;
135
136 // this is calculated at startup based on enabled features.
137 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
138 // this is the number of filled indexes in above array
139 static uint8_t activeBoxIdCount = 0;
140 // from mixer.c
141 extern float motor_disarmed[MAX_SUPPORTED_MOTORS];
142
143 // cause reboot after BST processing complete
144 static bool isRebootScheduled = false;
145
146 typedef struct box_e {
147 const uint8_t boxId; // see boxId_e
148 const char *boxName; // GUI-readable box name
149 const uint8_t permanentId; //
150 } box_t;
151
152 // FIXME remove ;'s
153 static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
154 { BOXARM, "ARM;", 0 },
155 { BOXANGLE, "ANGLE;", 1 },
156 { BOXHORIZON, "HORIZON;", 2 },
157 //{ BOXVARIO, "VARIO;", 4 },
158 { BOXMAG, "MAG;", 5 },
159 { BOXHEADFREE, "HEADFREE;", 6 },
160 { BOXHEADADJ, "HEADADJ;", 7 },
161 { BOXCAMSTAB, "CAMSTAB;", 8 },
162 { BOXPASSTHRU, "PASSTHRU;", 12 },
163 { BOXBEEPERON, "BEEPER;", 13 },
164 { BOXLEDLOW, "LEDLOW;", 15 },
165 { BOXCALIB, "CALIB;", 17 },
166 { BOXOSD, "OSD DISABLE SW;", 19 },
167 { BOXTELEMETRY, "TELEMETRY;", 20 },
168 { BOXSERVO1, "SERVO1;", 23 },
169 { BOXSERVO2, "SERVO2;", 24 },
170 { BOXSERVO3, "SERVO3;", 25 },
171 { BOXBLACKBOX, "BLACKBOX;", 26 },
172 { BOXFAILSAFE, "FAILSAFE;", 27 },
173 { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
174 };
175
176 extern uint8_t readData[BST_BUFFER_SIZE];
177 extern uint8_t writeData[BST_BUFFER_SIZE];
178
179 /*************************************************************************************************/
180 uint8_t writeBufferPointer = 1;
181 static void bstWrite8(uint8_t data) {
182 writeData[writeBufferPointer++] = data;
183 writeData[0] = writeBufferPointer;
184 }
185
186 static void bstWrite16(uint16_t data)
187 {
188 bstWrite8((uint8_t)(data >> 0));
189 bstWrite8((uint8_t)(data >> 8));
190 }
191
192 static void bstWrite32(uint32_t data)
193 {
194 bstWrite16((uint16_t)(data >> 0));
195 bstWrite16((uint16_t)(data >> 16));
196 }
197
198 uint8_t readBufferPointer = 4;
199 static uint8_t bstCurrentAddress(void)
200 {
201 return readData[0];
202 }
203
204 static uint8_t bstRead8(void)
205 {
206 return readData[readBufferPointer++] & 0xff;
207 }
208
209 static uint16_t bstRead16(void)
210 {
211 uint16_t t = bstRead8();
212 t += (uint16_t)bstRead8() << 8;
213 return t;
214 }
215
216 static uint32_t bstRead32(void)
217 {
218 uint32_t t = bstRead16();
219 t += (uint32_t)bstRead16() << 16;
220 return t;
221 }
222
223 static uint8_t bstReadDataSize(void)
224 {
225 return readData[1]-5;
226 }
227
228 static uint8_t bstReadCRC(void)
229 {
230 return readData[readData[1]+1];
231 }
232
233 static const box_t *findBoxByPermenantId(uint8_t permenantId)
234 {
235 uint8_t boxIndex;
236 const box_t *candidate;
237 for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) {
238 candidate = &boxes[boxIndex];
239 if (candidate->permanentId == permenantId) {
240 return candidate;
241 }
242 }
243 return NULL;
244 }
245
246 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
247
248 /*************************************************************************************************/
249 #define BST_USB_COMMANDS 0x0A
250 #define BST_GENERAL_HEARTBEAT 0x0B
251 #define BST_USB_DEVICE_INFO_REQUEST 0x04 //Handshake
252 #define BST_USB_DEVICE_INFO_FRAME 0x05 //Handshake
253 #define BST_READ_COMMANDS 0x26
254 #define BST_WRITE_COMMANDS 0x25
255 #define BST_PASSED 0x01
256 #define BST_FAILED 0x00
257
258 static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
259 {
260 uint32_t i, tmp, junk;
261
262 switch (bstRequest) {
263 case BST_API_VERSION:
264 bstWrite8(BST_PROTOCOL_VERSION);
265
266 bstWrite8(API_VERSION_MAJOR);
267 bstWrite8(API_VERSION_MINOR);
268 break;
269 case BST_BUILD_INFO:
270 for (i = 0; i < BUILD_DATE_LENGTH; i++) {
271 bstWrite8(buildDate[i]);
272 }
273 for (i = 0; i < BUILD_TIME_LENGTH; i++) {
274 bstWrite8(buildTime[i]);
275 }
276
277 for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
278 bstWrite8(shortGitRevision[i]);
279 }
280 break;
281 case BST_STATUS:
282 bstWrite16(getTaskDeltaTimeUs(TASK_PID));
283 #ifdef USE_I2C
284 bstWrite16(i2cGetErrorCounter());
285 #else
286 bstWrite16(0);
287 #endif
288 bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
289 // BST the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
290 // Requires new Multiwii protocol version to fix
291 // It would be preferable to setting the enabled bits based on BOXINDEX.
292 junk = 0;
293 tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
294 IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
295 IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
296 IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
297 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
298 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
299 IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
300 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
301 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
302 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
303 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
304 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
305 IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
306 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
307 IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
308 for (i = 0; i < activeBoxIdCount; i++) {
309 int flag = (tmp & (1 << activeBoxIds[i]));
310 if (flag)
311 junk |= 1 << i;
312 }
313 bstWrite32(junk);
314 bstWrite8(getCurrentPidProfileIndex());
315 break;
316 case BST_LOOP_TIME:
317 bstWrite16(getTaskDeltaTimeUs(TASK_PID));
318 break;
319 case BST_RC_TUNING:
320 bstWrite8(currentControlRateProfile->rcRates[FD_ROLL]);
321 bstWrite8(currentControlRateProfile->rcExpo[FD_ROLL]);
322 for (i = 0 ; i < 3; i++) {
323 bstWrite8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
324 }
325 bstWrite8(currentControlRateProfile->tpa_rate);
326 bstWrite8(currentControlRateProfile->thrMid8);
327 bstWrite8(currentControlRateProfile->thrExpo8);
328 bstWrite16(currentControlRateProfile->tpa_breakpoint);
329 bstWrite8(currentControlRateProfile->rcExpo[FD_YAW]);
330 bstWrite8(currentControlRateProfile->rcRates[FD_YAW]);
331 break;
332
333 case BST_PID:
334 for (i = 0; i < PID_ITEM_COUNT; i++) {
335 bstWrite8(currentPidProfile->pid[i].P);
336 bstWrite8(currentPidProfile->pid[i].I);
337 bstWrite8(currentPidProfile->pid[i].D);
338 }
339 pidInitConfig(currentPidProfile);
340 break;
341 case BST_MODE_RANGES:
342 for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
343 const modeActivationCondition_t *mac = modeActivationConditions(i);
344 const box_t *box = &boxes[mac->modeId];
345 bstWrite8(box->permanentId);
346 bstWrite8(mac->auxChannelIndex);
347 bstWrite8(mac->range.startStep);
348 bstWrite8(mac->range.endStep);
349 }
350 break;
351 case BST_MISC:
352 bstWrite16(rxConfig()->midrc);
353
354 bstWrite16(motorConfig()->minthrottle);
355 bstWrite16(motorConfig()->maxthrottle);
356 bstWrite16(motorConfig()->mincommand);
357
358 bstWrite16(failsafeConfig()->failsafe_throttle);
359
360 #ifdef USE_GPS
361 bstWrite8(gpsConfig()->provider); // gps_type
362 bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
363 bstWrite8(gpsConfig()->sbasMode); // gps_ubx_sbas
364 #else
365 bstWrite8(0); // gps_type
366 bstWrite8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
367 bstWrite8(0); // gps_ubx_sbas
368 #endif
369 bstWrite8(0); // legacy - was multiwiiCurrentMeterOutput);
370 bstWrite8(rxConfig()->rssi_channel);
371 bstWrite8(0);
372
373 bstWrite16(0); // was mag_declination / 10
374
375 bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
376 bstWrite8((batteryConfig()->vbatmincellvoltage + 5) / 10);
377 bstWrite8((batteryConfig()->vbatmaxcellvoltage + 5) / 10);
378 bstWrite8((batteryConfig()->vbatwarningcellvoltage + 5) / 10);
379 break;
380
381 case BST_FEATURE:
382 bstWrite32(featureMask());
383 break;
384
385 case BST_RX_CONFIG:
386 bstWrite8(rxConfig()->serialrx_provider);
387 bstWrite16(rxConfig()->maxcheck);
388 bstWrite16(rxConfig()->midrc);
389 bstWrite16(rxConfig()->mincheck);
390 bstWrite8(rxConfig()->spektrum_sat_bind);
391 bstWrite16(rxConfig()->rx_min_usec);
392 bstWrite16(rxConfig()->rx_max_usec);
393 break;
394
395 case BST_RX_MAP:
396 for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++)
397 bstWrite8(rxConfig()->rcmap[i]);
398 break;
399
400
401 #ifdef USE_LED_STRIP
402 case BST_LED_COLORS:
403 for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
404 bstWrite16(0);
405 bstWrite8(0);
406 bstWrite8(0);
407 }
408 break;
409
410 case BST_LED_STRIP_CONFIG:
411 for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
412 bstWrite32(0);
413 }
414 break;
415 #endif
416 case BST_DEADBAND:
417 bstWrite8(rcControlsConfig()->alt_hold_deadband);
418 bstWrite8(rcControlsConfig()->alt_hold_fast_change);
419 bstWrite8(rcControlsConfig()->deadband);
420 bstWrite8(rcControlsConfig()->yaw_deadband);
421 break;
422 case BST_FC_FILTERS:
423 bstWrite16(0);
424 break;
425 default:
426 // we do not know how to handle the (valid) message, indicate error BST
427 return false;
428 }
429 return true;
430 }
431
432 static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
433 {
434 uint32_t i;
435 uint16_t tmp;
436
437 bool ret = BST_PASSED;
438 switch (bstWriteCommand) {
439 case BST_SELECT_SETTING:
440 if (!ARMING_FLAG(ARMED)) {
441 changePidProfile(bstRead8());
442 }
443 break;
444 case BST_SET_LOOP_TIME:
445 bstRead16();
446 break;
447 case BST_SET_PID:
448 for (i = 0; i < PID_ITEM_COUNT; i++) {
449 currentPidProfile->pid[i].P = bstRead8();
450 currentPidProfile->pid[i].I = bstRead8();
451 currentPidProfile->pid[i].D = bstRead8();
452 }
453 break;
454 case BST_SET_RC_TUNING:
455 if (bstReadDataSize() >= 10) {
456 uint8_t rate;
457 currentControlRateProfile->rcRates[FD_ROLL] = bstRead8();
458 currentControlRateProfile->rcExpo[FD_ROLL] = bstRead8();
459 for (i = 0; i < 3; i++) {
460 currentControlRateProfile->rates[i] = bstRead8();
461 }
462 rate = bstRead8();
463 currentControlRateProfile->tpa_rate = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
464 currentControlRateProfile->thrMid8 = bstRead8();
465 currentControlRateProfile->thrExpo8 = bstRead8();
466 currentControlRateProfile->tpa_breakpoint = bstRead16();
467 if (bstReadDataSize() >= 11) {
468 currentControlRateProfile->rcExpo[FD_YAW] = bstRead8();
469 }
470 if (bstReadDataSize() >= 12) {
471 currentControlRateProfile->rcRates[FD_YAW] = bstRead8();
472 }
473 } else {
474 ret = BST_FAILED;
475 }
476 break;
477 case BST_SET_MODE_RANGE:
478 i = bstRead8();
479 if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
480 modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
481 i = bstRead8();
482 const box_t *box = findBoxByPermenantId(i);
483 if (box) {
484 mac->modeId = box->boxId;
485 mac->auxChannelIndex = bstRead8();
486 mac->range.startStep = bstRead8();
487 mac->range.endStep = bstRead8();
488
489 rcControlsInit();
490 } else {
491 ret = BST_FAILED;
492 }
493 } else {
494 ret = BST_FAILED;
495 }
496 break;
497 case BST_SET_MISC:
498 tmp = bstRead16();
499 if (tmp < 1600 && tmp > 1400)
500 rxConfigMutable()->midrc = tmp;
501
502 motorConfigMutable()->minthrottle = bstRead16();
503 motorConfigMutable()->maxthrottle = bstRead16();
504 motorConfigMutable()->mincommand = bstRead16();
505
506 failsafeConfigMutable()->failsafe_throttle = bstRead16();
507
508 #ifdef USE_GPS
509 gpsConfigMutable()->provider = bstRead8(); // gps_type
510 bstRead8(); // gps_baudrate
511 gpsConfigMutable()->sbasMode = bstRead8(); // gps_ubx_sbas
512 #else
513 bstRead8(); // gps_type
514 bstRead8(); // gps_baudrate
515 bstRead8(); // gps_ubx_sbas
516 #endif
517 bstRead8(); // legacy - was multiwiiCurrentMeterOutput
518 rxConfigMutable()->rssi_channel = bstRead8();
519 bstRead8();
520
521 bstRead16(); // was mag_declination / 10
522
523 voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended
524 batteryConfigMutable()->vbatmincellvoltage = bstRead8() * 10; // vbatlevel_warn1 in MWC2.3 GUI
525 batteryConfigMutable()->vbatmaxcellvoltage = bstRead8() * 10; // vbatlevel_warn2 in MWC2.3 GUI
526 batteryConfigMutable()->vbatwarningcellvoltage = bstRead8() * 10; // vbatlevel when buzzer starts to alert
527 break;
528
529 #if defined(USE_ACC)
530 case BST_ACC_CALIBRATION:
531 if (!ARMING_FLAG(ARMED))
532 accStartCalibration();
533 break;
534 #endif
535
536 #if defined(USE_MAG)
537 case BST_MAG_CALIBRATION:
538 if (!ARMING_FLAG(ARMED)) {
539 compassStartCalibration();
540 }
541 #endif
542
543 break;
544 case BST_EEPROM_WRITE:
545 if (ARMING_FLAG(ARMED)) {
546 ret = BST_FAILED;
547 bstWrite8(ret);
548 return ret;
549 }
550 writeEEPROM();
551 readEEPROM();
552 break;
553 case BST_SET_FEATURE:
554 featureConfigReplace(bstRead32()); // features bitmap
555 #ifdef SERIALRX_UART
556 if (featureIsEnabled(FEATURE_RX_SERIAL)) {
557 serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL;
558 } else {
559 serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE;
560 }
561 #endif
562 break;
563 case BST_SET_RX_CONFIG:
564 rxConfigMutable()->serialrx_provider = bstRead8();
565 rxConfigMutable()->maxcheck = bstRead16();
566 rxConfigMutable()->midrc = bstRead16();
567 rxConfigMutable()->mincheck = bstRead16();
568 rxConfigMutable()->spektrum_sat_bind = bstRead8();
569 if (bstReadDataSize() > 8) {
570 rxConfigMutable()->rx_min_usec = bstRead16();
571 rxConfigMutable()->rx_max_usec = bstRead16();
572 }
573 break;
574 case BST_SET_RX_MAP:
575 for (i = 0; i < RX_MAPPABLE_CHANNEL_COUNT; i++) {
576 rxConfigMutable()->rcmap[i] = bstRead8();
577 }
578 break;
579
580 #ifdef USE_LED_STRIP
581 case BST_SET_LED_COLORS:
582 //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
583 {
584 bstRead8();
585 bstRead16();
586 bstRead8();
587 bstRead8();
588 }
589 break;
590 case BST_SET_LED_STRIP_CONFIG:
591 {
592 i = bstRead8();
593 if (i >= LED_MAX_STRIP_LENGTH || bstReadDataSize() != (1 + 4)) {
594 ret = BST_FAILED;
595 break;
596 }
597 #if defined(USE_LED_STRIP_STATUS_MODE)
598 ledConfig_t *ledConfig = &ledStripStatusModeConfigMutable()->ledConfigs[i];
599 *ledConfig = bstRead32();
600 reevaluateLedConfig();
601 #endif
602 }
603 break;
604 #endif
605 case BST_REBOOT:
606 isRebootScheduled = true;
607 break;
608 case BST_DISARM:
609 if (ARMING_FLAG(ARMED)) {
610 disarm(DISARM_REASON_SERIAL_COMMAND);
611 }
612 setArmingDisabled(ARMING_DISABLED_BST);
613 break;
614 case BST_ENABLE_ARM:
615 unsetArmingDisabled(ARMING_DISABLED_BST);
616 break;
617 case BST_SET_DEADBAND:
618 rcControlsConfigMutable()->alt_hold_deadband = bstRead8();
619 rcControlsConfigMutable()->alt_hold_fast_change = bstRead8();
620 rcControlsConfigMutable()->deadband = bstRead8();
621 rcControlsConfigMutable()->yaw_deadband = bstRead8();
622 break;
623 case BST_SET_FC_FILTERS:
624 bstRead16(); // 1KHz sampling no longer supported
625 break;
626
627 default:
628 // we do not know how to handle the (valid) message, indicate error BST
629 ret = BST_FAILED;
630 }
631 bstWrite8(ret);
632
633 if (ret == BST_FAILED)
634 return false;
635
636 return true;
637 }
638
639 static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
640 {
641 bstWrite8(BST_USB_DEVICE_INFO_FRAME); //Sub CPU Device Info FRAME
642 bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
643 bstWrite8(0x00);
644 bstWrite8(0x00);
645 bstWrite8(0x00);
646 bstWrite8(FC_VERSION_MAJOR); //Firmware ID
647 bstWrite8(FC_VERSION_MINOR); //Firmware ID
648 bstWrite8(0x00);
649 bstWrite8(0x00);
650 return true;
651 }
652
653 /*************************************************************************************************/
654 #define BST_RESET_TIME 1.2*1000*1000 //micro-seconds
655
656 uint32_t resetBstTimer = 0;
657 bool needResetCheck = true;
658
659 extern bool cleanflight_data_ready;
660
661 void bstProcessInCommand(void)
662 {
663 readBufferPointer = 2;
664 if (bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) {
665 if (bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) {
666 uint8_t i;
667 writeBufferPointer = 1;
668 cleanflight_data_ready = false;
669 for (i = 0; i < BST_BUFFER_SIZE; i++) {
670 writeData[i] = 0;
671 }
672 switch (bstRead8()) {
673 case BST_USB_DEVICE_INFO_REQUEST:
674 bstRead8();
675 if (bstSlaveUSBCommandFeedback(/*bstRead8()*/))
676 coreProReady = true;
677 break;
678 case BST_READ_COMMANDS:
679 bstWrite8(BST_READ_COMMANDS);
680 bstSlaveProcessFeedbackCommand(bstRead8());
681 break;
682 case BST_WRITE_COMMANDS:
683 bstWrite8(BST_WRITE_COMMANDS);
684 bstSlaveProcessWriteCommand(bstRead8());
685 break;
686 default:
687 // we do not know how to handle the (valid) message, indicate error BST
688 break;
689 }
690 cleanflight_data_ready = true;
691 }
692 } else if (bstCurrentAddress() == 0x00) {
693 if (bstReadCRC() == CRC8 && bstRead8()==BST_GENERAL_HEARTBEAT) {
694 resetBstTimer = micros();
695 needResetCheck = true;
696 }
697 }
698 }
699
700 static void resetBstChecker(timeUs_t currentTimeUs)
701 {
702 if (needResetCheck) {
703 if (currentTimeUs >= (resetBstTimer + BST_RESET_TIME))
704 {
705 bstTimeoutUserCallback();
706 needResetCheck = false;
707 }
708 }
709 }
710
711 /*************************************************************************************************/
712 #define UPDATE_AT_02HZ ((1000 * 1000) / 2)
713 static uint32_t next02hzUpdateAt_1 = 0;
714
715 #define UPDATE_AT_20HZ ((1000 * 1000) / 20)
716 static uint32_t next20hzUpdateAt_1 = 0;
717
718 static uint8_t sendCounter = 0;
719
720 void taskBstMasterProcess(timeUs_t currentTimeUs)
721 {
722 if (coreProReady) {
723 if (currentTimeUs >= next02hzUpdateAt_1 && !bstWriteBusy()) {
724 writeFCModeToBST();
725 next02hzUpdateAt_1 = currentTimeUs + UPDATE_AT_02HZ;
726 }
727 if (currentTimeUs >= next20hzUpdateAt_1 && !bstWriteBusy()) {
728 if (sendCounter == 0)
729 writeRCChannelToBST();
730 else if (sendCounter == 1)
731 writeRollPitchYawToBST();
732 sendCounter++;
733 if (sendCounter > 1)
734 sendCounter = 0;
735 next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ;
736 }
737 #ifdef USE_GPS
738 if (sensors(SENSOR_GPS) && !bstWriteBusy())
739 writeGpsPositionPrameToBST();
740 #endif
741
742 }
743 bstMasterWriteLoop();
744 if (isRebootScheduled) {
745 stopMotors();
746 systemReset();
747 }
748 resetBstChecker(currentTimeUs);
749 }
750
751 /*************************************************************************************************/
752 static uint8_t masterWriteBufferPointer;
753 static uint8_t masterWriteData[BST_BUFFER_SIZE];
754
755 static void bstMasterStartBuffer(uint8_t address)
756 {
757 masterWriteData[0] = address;
758 masterWriteBufferPointer = 2;
759 }
760
761 static void bstMasterWrite8(uint8_t data)
762 {
763 masterWriteData[masterWriteBufferPointer++] = data;
764 masterWriteData[1] = masterWriteBufferPointer;
765 }
766
767 static void bstMasterWrite16(uint16_t data)
768 {
769 bstMasterWrite8((uint8_t)(data >> 8));
770 bstMasterWrite8((uint8_t)(data >> 0));
771 }
772
773 /*************************************************************************************************/
774 #define PUBLIC_ADDRESS 0x00
775
776 #ifdef USE_GPS
777 static void bstMasterWrite32(uint32_t data)
778 {
779 bstMasterWrite16((uint16_t)(data >> 16));
780 bstMasterWrite16((uint16_t)(data >> 0));
781 }
782
783 static int32_t lat = 0;
784 static int32_t lon = 0;
785 static uint16_t altM = 0;
786 static uint8_t numOfSat = 0;
787 #endif
788
789 #ifdef USE_GPS
790 bool writeGpsPositionPrameToBST(void)
791 {
792 if ((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (altM != (gpsSol.llh.altCm / 100)) || (numOfSat != gpsSol.numSat)) {
793 lat = gpsSol.llh.lat;
794 lon = gpsSol.llh.lon;
795 altM = gpsSol.llh.altCm / 100;
796 numOfSat = gpsSol.numSat;
797 uint16_t speed = (gpsSol.groundSpeed * 9 / 25);
798 uint16_t gpsHeading = 0;
799 uint16_t altitude = 0;
800 gpsHeading = gpsSol.groundCourse * 10;
801 altitude = altM + 1000; // To be verified: in m +1000m offset for neg. altitudes?
802
803 bstMasterStartBuffer(PUBLIC_ADDRESS);
804 bstMasterWrite8(GPS_POSITION_FRAME_ID);
805 bstMasterWrite32(lat);
806 bstMasterWrite32(lon);
807 bstMasterWrite16(speed);
808 bstMasterWrite16(gpsHeading);
809 bstMasterWrite16(altitude);
810 bstMasterWrite8(numOfSat);
811 bstMasterWrite8(0x00);
812
813 return bstMasterWrite(masterWriteData);
814 } else
815 return false;
816 }
817 #endif
818
819 bool writeRollPitchYawToBST(void)
820 {
821 int16_t X = -attitude.values.pitch * (M_PIf / 1800.0f) * 10000;
822 int16_t Y = attitude.values.roll * (M_PIf / 1800.0f) * 10000;
823 int16_t Z = 0;//radiusHeading * 10000;
824
825 bstMasterStartBuffer(PUBLIC_ADDRESS);
826 bstMasterWrite8(FC_ATTITUDE_FRAME_ID);
827 bstMasterWrite16(X);
828 bstMasterWrite16(Y);
829 bstMasterWrite16(Z);
830
831 return bstMasterWrite(masterWriteData);
832 }
833
834 bool writeRCChannelToBST(void)
835 {
836 uint8_t i = 0;
837 bstMasterStartBuffer(PUBLIC_ADDRESS);
838 bstMasterWrite8(RC_CHANNEL_FRAME_ID);
839 for (i = 0; i < (USABLE_TIMER_CHANNEL_COUNT-1); i++) {
840 bstMasterWrite16(rcData[i]);
841 }
842
843 return bstMasterWrite(masterWriteData);
844 }
845
846 bool writeFCModeToBST(void)
847 {
848 uint8_t tmp = 0;
849 tmp = IS_ENABLED(ARMING_FLAG(ARMED)) |
850 IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 |
851 IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << 2 |
852 IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
853 IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
854 IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
855
856 bstMasterStartBuffer(PUBLIC_ADDRESS);
857 bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
858 bstMasterWrite8(tmp);
859 bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
860
861 return bstMasterWrite(masterWriteData);
862 }
863 /*************************************************************************************************/