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