├── AdjustableVariables └── AdjustableVariables.ino ├── LCDRemote ├── LCDRemote.ino ├── MemoryFree.cpp ├── MemoryFree.h └── filter.h ├── LICENSE ├── MimicControl ├── MimicControl.ino ├── MimicControl.pdf └── filter.h ├── README.md ├── SerialControl └── SerialControl.ino ├── ServoOut └── ServoOut.ino ├── SpeedAngleControl └── SpeedAngleControl.ino └── libraries ├── SBGC_Arduino ├── SBGC_Arduino.cpp └── SBGC_Arduino.h └── SBGC_lib ├── SBGC.h ├── SBGC_cmd_helpers.cpp └── include ├── SBGC_adj_vars.h ├── SBGC_cmd_helpers.h ├── SBGC_command.h ├── SBGC_parser.h └── SBGC_rc.h /AdjustableVariables/AdjustableVariables.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This is example sketch for Arduino, that 3 | shows how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Demo: Change gimbal's parameters in real-time using Adjustable Variables 7 | 8 | Arduino hardware: 9 | - analog potentiometer on the pin A1 (connect GND, +5V to its side outputs) 10 | 11 | Gimbal settings: 12 | - RC control in SPEED mode, RC signal should come from active RC source 13 | 14 | Copyright (c) 2014 Aleksei Moskalenko 15 | *******************************************************************************/ 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | 22 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 23 | #define SERIAL_SPEED 115200 24 | 25 | // Set serial port where SBGC32 is connected 26 | #define serial Serial 27 | 28 | 29 | void setup() { 30 | serial.begin(SERIAL_SPEED); 31 | SBGC_Demo_setup(&serial); 32 | 33 | // Take a pause to let gimbal controller to initialize 34 | delay(3000); 35 | } 36 | 37 | void loop() { 38 | SBGC_cmd_set_adj_vars_var_t adj_vars[] = { 39 | { ADJ_VAR_RC_SPEED_ROLL, 0 }, 40 | { ADJ_VAR_RC_SPEED_PITCH, 0 }, 41 | { ADJ_VAR_RC_SPEED_YAW, 0 } 42 | }; 43 | 44 | 45 | blink_led(1); 46 | ////////////// Demo1: Gradually increase the speed of RC control for all axes 47 | // You can see the speed changes by applying RC control from you remote or joystick 48 | for(int i=0; i<100; i++) { 49 | adj_vars[0].val = i; 50 | adj_vars[1].val = i; 51 | adj_vars[2].val = i; 52 | 53 | SBGC_cmd_set_adj_vars_send(adj_vars, 3, sbgc_parser); 54 | delay(100); // 100*100 = 10 seconds to finish demo 55 | } 56 | delay(1000); 57 | 58 | 59 | blink_led(2); 60 | ////////////// Demo2: Use the value from analog potentiometer to control speed of RC control for all axes 61 | // You can see the speed changes by applying RC control from you remote or joystick 62 | for(int i=0; i<500; i++) { 63 | uint8_t speed = (analogRead(1)>>2); // range 0..1023 -> 0..255 64 | adj_vars[0].val = speed; 65 | adj_vars[1].val = speed; 66 | adj_vars[2].val = speed; 67 | 68 | SBGC_cmd_set_adj_vars_send(adj_vars, 3, sbgc_parser); 69 | delay(20); // 20*500 = 10 seconds to finish demo 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /LCDRemote/LCDRemote.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This is example sketch for Arduino, that 3 | shows how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Demo: This is a skeleton for LCD-enabled remote controller for SBGC32-powered gimbal. 7 | Tested with the Arduino UNO and Arduino 1.0 IDE. 8 | 9 | Basic functions: 10 | - Display system status using multiple pages (Up/Down arrow to scroll): 11 | - battery voltage, 12 | - active profile 13 | - average error of stabilization in 0.001 degrees 14 | - communication errors, I2C errors, various debug information 15 | 16 | - Display a customizable set of adjustable variables (Left/Right arrow or encoder Push-button to scroll), 17 | and change their values by the rotary encoder; 18 | - Trim ROLL axis with 0.1 degree precision 19 | 20 | - Use analog joystick to control PITCH and YAW axis (passed to SBGC controller as regular RC channels) 21 | - joystick push-button acts as "Menu" button 22 | 23 | - Navigation "select" button turns motors ON/OFF 24 | 25 | Arduino hardware: 26 | - Arduino UNO 27 | - LCD Key Shield from elfreaks, that includes: 28 | - 1602 LCD display 29 | - Rotary encoder with push button 30 | - 5 navigation buttons (Left,Right,Up,Down,Select) 31 | http://www.elecfreaks.com/wiki/index.php?title=LCD_Key_Shield 32 | - 2-axis joystick with push button 33 | - Wireless serial connection (optional) 34 | 35 | Gimbal settings: 36 | - Firmware version 2.55b7 or above 37 | - RC control in SPEED or ANGLE mode 38 | - Assign desired actions to menu button in the "Service" tab 39 | 40 | !!!WARNING!!! Its recommended to increase the serial buffer size to the max. size of a command you want to receive. 41 | Default buffer is 64 bytes, that is not enough to fit incoming SBGC API commands. 42 | For this example, 150 bytes works well. 43 | You can do it in the HardwareSerial.cpp (depends on Arduino IDE) 44 | 45 | 46 | Copyright (c) 2014-2015 Aleksei Moskalenko 47 | *******************************************************************************/ 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include "filter.h" 53 | #include "MemoryFree.h" 54 | 55 | 56 | /************************************************************************************ 57 | * Hardware Configuration 58 | *************************************************************************************/ 59 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 60 | #define SERIAL_SPEED 115200 // Default is 115200 61 | //#define SOFTWARE_SERIAL_RX_PIN 11 // define to use software serial instead of hardware serial 62 | //#define SOFTWARE_SERIAL_TX_PIN 12 // (but actually, it does not work for unknown reason) 63 | #define ENCODER_A_PIN 3 // Incremental Encoder singal A is PD3 64 | #define ENCODER_B_PIN 2 // Incremental Encoder singal B is PD2 65 | #define LCD_BACLIGHT_PIN 10 // pin to toggle back-light ON 66 | #define LCD_ROWS 2 67 | #define LCD_COLS 16 68 | #define NAV_BTN_ANALOG_PIN 0 // 6 navigation buttons are connected to single analog input by dividers 69 | #define LCD_SHIELD_VER 11 // version of LCD shield (from elfreaks) 70 | #define JOY_X_ANALOG_PIN 1 // joystick X-axis input 71 | #define JOY_Y_ANALOG_PIN 2 // joystick Y-axis input 72 | #define JOY_BTN_PIN 11 // joystick button input 73 | #define LCD_RS_PIN 8 74 | #define LCD_ENABLE_PIN 9 75 | #define LCD_D4_PIN 4 76 | #define LCD_D5_PIN 5 77 | #define LCD_D6_PIN 6 78 | #define LCD_D7_PIN 7 79 | /************************************************************************************ 80 | * Serial wireless connection. Remove (comment) definitions in this block if you want only wire connection. 81 | *************************************************************************************/ 82 | #define BLUETOOTH_CONNECTION 83 | // Choose (uncomment) one of the supported modules, or implement bt_master_connect() for your own module. 84 | //#define BLUETOOTH_RN42 // RN-42. 85 | //#define BLUETOOTH_HM10 // HM-10, HM-11 (BLE). Implement bt_master_connect() for your own module. 86 | #define BLUETOOTH_HC05 // Be sure to use HC-05 module with KEY pin. This pin shuld be pulled to 3.3V 87 | #define BLUETOOTH_CLIENT_MAC_ADDR "98D3,34,90DB5E" // If defined, use this MAC address to connect (12 hex digits), instead of searching | If HC-05 is used write MAC address in following format: "XXXX,XX,XXXXXX" 88 | //#define BLUETOOTH_CLIENT_NAME_PATTERN "SBGC" // If defined, will search for a device containing this pattern in its name 89 | #define BLUETOOTH_CLIENT_PIN "1234" // PIN code set for connection 90 | #define BLUETOOTH_BAUD 38400 91 | #define BLUETOOTH_DO_SETUP // configure BT module as master role and set PIN. May be done only once. 92 | #define BLUETOOTH_BUF_SIZE 60 // size of buffer for answers from module 93 | #define BLUETOOTH_DEBUG false // set to true to display answers from BT module 94 | #if(defined(BLUETOOTH_HM10)) 95 | #define BLE_MODE // BLE requires special mode: 20-byte packets with pause between them. Tx is not yet implemented in this example 96 | #endif 97 | /************************************************************************************* 98 | * Timings Configuration 99 | *************************************************************************************/ 100 | #ifdef BLE_MODE 101 | #define REALTIME_DATA_REQUEST_INTERAL_MS 500 // interval between reatime data requests 102 | #define JOYSTICK_CMD_SEND_INTERVAL_MS 50 // interval of sending joystick control commands 103 | #else 104 | #define REALTIME_DATA_REQUEST_INTERAL_MS 100 // interval between reatime data requests 105 | #define JOYSTICK_CMD_SEND_INTERVAL_MS 50 // interval of sending joystick control commands 106 | #endif 107 | #define MAX_WAIT_TIME_MS 2000 // time to wait for incoming commands to be in CONNECTED state 108 | #define LOW_RATE_TASK_INTERVAL 500 // interval between low-priority tasks (display update, etc) 109 | #define BTN_BOUNCE_THRESHOLD_MS 30 // interval for button de-bouncer threshold 110 | /*************************************************************************************/ 111 | 112 | 113 | 114 | #define BTN_STATE_RELEASED 0 115 | #define BTN_STATE_PRESSED 1 116 | 117 | /* Navigation buttons */ 118 | enum { 119 | NAV_BTN_RIGHT=1, 120 | NAV_BTN_UP, 121 | NAV_BTN_DOWN, 122 | NAV_BTN_LEFT, 123 | NAV_BTN_SELECT, 124 | NAV_BTN_ENCODER_SELECT 125 | }; 126 | 127 | 128 | 129 | /* Custom types of adjustable variables. Take any big ID that is not used by the SBGC API */ 130 | #define ADJ_VAR_CUSTOM_ID 200 131 | enum { 132 | ADJ_VAR_ROLL_TRIM = ADJ_VAR_CUSTOM_ID, 133 | }; 134 | 135 | /* A set of adjustable variables that can be changed by encoder knob. 136 | * You may add any variables listed in the SBGC_adj_vars.h 137 | * Be carefull, this structure is placed in to the RAM, that may be a problem with the low memory for boards like UNO 138 | */ 139 | adjustable_var_t adj_vars[] = { 140 | { { ADJ_VAR_ROLL_TRIM, "ROLL_TRIM", -900, 900 }, 1, 0, 0 }, 141 | { ADJ_VAR_DEF_RC_SPEED_PITCH, 1, 0, 0 }, 142 | { ADJ_VAR_DEF_RC_SPEED_YAW, 1, 0, 0 }, 143 | { ADJ_VAR_DEF_FOLLOW_SPEED_PITCH, 1, 0, 0 }, 144 | { ADJ_VAR_DEF_FOLLOW_SPEED_YAW, 1, 0, 0 }, 145 | { ADJ_VAR_DEF_FOLLOW_LPF_PITCH, 1, 0, 0 }, 146 | { ADJ_VAR_DEF_FOLLOW_LPF_YAW, 1, 0, 0 }, 147 | { ADJ_VAR_DEF_FOLLOW_DEADBAND, 1, 0, 0 }, 148 | }; 149 | 150 | #define ADJ_VARS_NUM (sizeof(adj_vars)/sizeof(adjustable_var_t)) 151 | 152 | 153 | // Functions prototypes 154 | inline void process_cmd_realtime_data(); 155 | inline void process_in_queue(); 156 | void encoder_irq_handler(); 157 | inline uint8_t read_nav_buttons_state(); 158 | inline void update_display(); 159 | void set_connected(); 160 | void request_adj_vars_val(); 161 | void set_local_adj_var(uint8_t id, int32_t val); 162 | inline void bt_master_connect(); 163 | 164 | // Global variables 165 | static LiquidCrystal lcd(LCD_RS_PIN, LCD_ENABLE_PIN, LCD_D4_PIN, LCD_D5_PIN, LCD_D6_PIN, LCD_D7_PIN); 166 | static uint8_t cur_adj_var_idx = 0; 167 | static SBGC_cmd_realtime_data_t rt_data; 168 | static uint16_t cur_time_ms, low_rate_time_ms, last_cmd_time_ms, rt_req_last_time_ms, joy_last_time_ms; //last_bt_connect_ms; 169 | static uint8_t is_connected = 0; 170 | static avg_var16 target_error_avg; 171 | static btn_state_t nav_btn, joy_btn; 172 | static int8_t cur_page = 0; 173 | static int16_t debug1, debug2, debug3, debug4, free_memory; 174 | static SBGC_cmd_control_ext_t cmd_control = { 175 | { SBGC_CONTROL_MODE_ANGLE, SBGC_CONTROL_MODE_RC, SBGC_CONTROL_MODE_RC }, // control mode for ROLL, PITCH, YAW 176 | { { 0, 0 }, { 0, 0 }, { 0, 0 } } // angle and speed 177 | }; 178 | static int16_t joy_x_neutral, joy_y_neutral; 179 | static uint8_t need_update_display; 180 | 181 | 182 | 183 | #ifdef SOFTWARE_SERIAL_RX_PIN 184 | #include 185 | SoftwareSerial serial(SOFTWARE_SERIAL_RX_PIN, SOFTWARE_SERIAL_TX_PIN); 186 | #else 187 | // Set serial port where SBGC32 is connected 188 | #define serial Serial 189 | #endif 190 | 191 | 192 | 193 | 194 | void setup() { 195 | serial.begin(SERIAL_SPEED); 196 | SBGC_Demo_setup(&serial); 197 | 198 | /////// Setup LCD 199 | lcd.begin(LCD_COLS, LCD_ROWS); 200 | // enable back light 201 | pinMode(LCD_BACLIGHT_PIN, OUTPUT); 202 | digitalWrite(LCD_BACLIGHT_PIN, 1); 203 | 204 | 205 | ////// Setup encoder 206 | pinMode(ENCODER_A_PIN, INPUT); 207 | pinMode(ENCODER_B_PIN, INPUT); 208 | digitalWrite(ENCODER_A_PIN, 1); 209 | digitalWrite(ENCODER_B_PIN, 1); 210 | attachInterrupt(1, encoder_irq_handler, FALLING); //interrupts: numbers 0 (on digital pin 2) and 1 (on digital pin 3). 211 | 212 | /////// Analog joystick 213 | pinMode(JOY_BTN_PIN, INPUT); 214 | digitalWrite(JOY_BTN_PIN, 1); // pulled up to VCC, button will short it to GND (i,e, pressed state = LOW) 215 | joy_x_neutral = analogRead(JOY_X_ANALOG_PIN); 216 | joy_y_neutral = analogRead(JOY_Y_ANALOG_PIN); 217 | 218 | target_error_avg.init(4); 219 | 220 | bt_master_connect(); 221 | 222 | // We are ready 223 | //blink_led(3); 224 | } 225 | 226 | 227 | // Main loop 228 | void loop() { 229 | cur_time_ms = millis(); 230 | 231 | process_in_queue(); 232 | 233 | ////////// Request realtime data with the fixed rate 234 | if((cur_time_ms - rt_req_last_time_ms) > REALTIME_DATA_REQUEST_INTERAL_MS) { 235 | SerialCommand cmd; 236 | if(is_connected) { 237 | cmd.init(SBGC_CMD_REALTIME_DATA_4); 238 | } else { // Set version request to init connection 239 | cmd.init(SBGC_CMD_BOARD_INFO); 240 | #ifdef BLE_MODE 241 | cmd.writeWord(10); // pause between packets, ms 242 | #endif 243 | } 244 | 245 | sbgc_parser.send_cmd(cmd, 0); 246 | 247 | rt_req_last_time_ms = cur_time_ms; 248 | } 249 | 250 | 251 | ////////// Process navigation 252 | if(debounce_button(nav_btn, read_nav_buttons_state())) { 253 | switch(nav_btn.trigger_state) { 254 | case NAV_BTN_RIGHT: // select next adj. var 255 | case NAV_BTN_ENCODER_SELECT: 256 | cur_adj_var_idx = (cur_adj_var_idx+1)%ADJ_VARS_NUM; 257 | break; 258 | 259 | case NAV_BTN_LEFT: // select prev adj. var 260 | cur_adj_var_idx = (cur_adj_var_idx + (ADJ_VARS_NUM - 1))%ADJ_VARS_NUM; 261 | break; 262 | 263 | case NAV_BTN_UP: // select next page 264 | cur_page++; 265 | break; 266 | 267 | case NAV_BTN_DOWN: // select prev page 268 | cur_page--; 269 | break; 270 | 271 | case NAV_BTN_SELECT: // turn motors ON/OFF 272 | SBGC_cmd_execute_menu_send(SBGC_MENU_MOTOR_TOGGLE, sbgc_parser); 273 | break; 274 | 275 | } 276 | // update display immediately to reduce lag 277 | need_update_display = 1; 278 | } 279 | 280 | 281 | 282 | ////////// Send the value of updated adj. variables to the board 283 | for(uint8_t i=0; i JOYSTICK_CMD_SEND_INTERVAL_MS) { 308 | cmd_control.data[PITCH].angle = analogRead(JOY_Y_ANALOG_PIN) - joy_y_neutral; 309 | cmd_control.data[YAW].angle = analogRead(JOY_X_ANALOG_PIN) - joy_x_neutral; 310 | SBGC_cmd_control_ext_send(cmd_control, sbgc_parser); 311 | 312 | joy_last_time_ms = cur_time_ms; 313 | } 314 | 315 | 316 | 317 | //////////// Process joystick button and send it as a "menu" cmd press to the board 318 | if(debounce_button(joy_btn, digitalRead(JOY_BTN_PIN) == LOW ? BTN_STATE_PRESSED : BTN_STATE_RELEASED)) { 319 | if(joy_btn.trigger_state == BTN_STATE_PRESSED) { 320 | SBGC_cmd_execute_menu_send(SBGC_MENU_BUTTON_PRESS, sbgc_parser); 321 | } 322 | } 323 | 324 | 325 | /////////// Low-rate tasks 326 | if((cur_time_ms - low_rate_time_ms) > LOW_RATE_TASK_INTERVAL || need_update_display) { 327 | // update LCD to display animation and state 328 | update_display(); 329 | 330 | low_rate_time_ms = cur_time_ms; 331 | need_update_display = 0; 332 | } 333 | 334 | // Try to restore BT connection 335 | /* 336 | if(!is_connected && (cur_time_ms - last_bt_connect_ms) > MAX_WAIT_TIME_MS*2) { 337 | bt_master_connect(); 338 | } 339 | */ 340 | 341 | 342 | 343 | free_memory = freeMemory(); 344 | } 345 | 346 | // Called once on a connection established 347 | void set_connected() { 348 | is_connected = 1; 349 | request_adj_vars_val(); 350 | } 351 | 352 | 353 | // Process incoming commands. Call it as frequently as possible, to prevent overrun of serial input buffer. 354 | void process_in_queue() { 355 | while(sbgc_parser.read_cmd()) { 356 | SerialCommand &cmd = sbgc_parser.in_cmd; 357 | last_cmd_time_ms = cur_time_ms; 358 | if(!is_connected) set_connected(); 359 | 360 | uint8_t error = 0; 361 | 362 | switch(cmd.id) { 363 | // Receive realtime data 364 | case SBGC_CMD_REALTIME_DATA_3: 365 | case SBGC_CMD_REALTIME_DATA_4: 366 | error = SBGC_cmd_realtime_data_unpack(rt_data, cmd); 367 | if(!error) { 368 | // Extract some usefull data 369 | // Average stabilization error (0.001 degree) 370 | uint32_t err = (uint32_t)(abs(rt_data.imu_angle[ROLL] - rt_data.target_angle[ROLL]) 371 | + abs(rt_data.imu_angle[PITCH] - rt_data.target_angle[PITCH]) 372 | + abs(rt_data.imu_angle[YAW] - rt_data.target_angle[YAW])) * (uint32_t)(SBGC_ANGLE_DEGREE_SCALE*1000); 373 | 374 | target_error_avg.average(min(err, 999)); 375 | } else { 376 | sbgc_parser.onParseError(error); 377 | } 378 | break; 379 | 380 | 381 | // Receive the actual values of adjustable variables 382 | case SBGC_CMD_SET_ADJ_VARS_VAL: 383 | { 384 | SBGC_cmd_set_adj_vars_var_t buf[ADJ_VARS_NUM]; 385 | uint8_t vars_num = ADJ_VARS_NUM; 386 | error = SBGC_cmd_set_adj_vars_unpack(buf, vars_num, cmd); 387 | if(!error) { 388 | // Assign received values to our local set of variables 389 | for(uint8_t i=0; i MAX_WAIT_TIME_MS) { 403 | is_connected = 0; 404 | //last_bt_connect_ms = cur_time_ms; 405 | } 406 | } 407 | 408 | 409 | /* 410 | * Request the actual values of adjustable variables from the board 411 | */ 412 | void request_adj_vars_val() { 413 | // Filter only variables that board understands 414 | uint8_t ids[ADJ_VARS_NUM]; 415 | uint8_t vars_num = 0; 416 | for(uint8_t i=0; i BTN_BOUNCE_THRESHOLD_MS) { 465 | btn.trigger_state = btn.state; 466 | return 1; 467 | } 468 | return 0; 469 | } 470 | 471 | 472 | // Reads the state of buttons connected to A0 473 | // (this is hardware-specific) 474 | uint8_t read_nav_buttons_state() { 475 | uint16_t adc_key_in = analogRead(NAV_BTN_ANALOG_PIN); 476 | 477 | if (adc_key_in > 1000) return BTN_STATE_RELEASED; 478 | 479 | // For V1.1 us this threshold 480 | #if(LCD_SHIELD_VER == 11) 481 | if (adc_key_in < 50) return NAV_BTN_LEFT; 482 | if (adc_key_in < 150) return NAV_BTN_UP; 483 | if (adc_key_in < 250) return NAV_BTN_RIGHT; 484 | if (adc_key_in < 450) return NAV_BTN_SELECT; 485 | if (adc_key_in < 700) return NAV_BTN_DOWN; 486 | if (adc_key_in < 850) return NAV_BTN_ENCODER_SELECT; 487 | #elif(LCD_SHIELD_VER == 10) 488 | // For V1.0 use the one below: 489 | if (adc_key_in < 50) return NAV_BTN_RIGHT; 490 | if (adc_key_in < 195) return NAV_BTN_UP; 491 | if (adc_key_in < 380) return NAV_BTN_DOWN; 492 | if (adc_key_in < 555) return NAV_BTN_LEFT; 493 | if (adc_key_in < 790) return NAV_BTN_SELECT; 494 | #else 495 | #error "Unknown LCD shield version" 496 | #endif 497 | 498 | return BTN_STATE_RELEASED; // when all others fail, return this... 499 | } 500 | 501 | // Shows progress animation 502 | inline void lcd_print_progress(uint8_t &cursor_pos) { 503 | for(uint8_t i=0; i<3; i++) { 504 | cursor_pos+= lcd.print(((uint8_t)(cur_time_ms/1000)) % 4 > i ? '.' : ' '); 505 | } 506 | } 507 | // Finish a display raw by space characters 508 | inline void lcd_fill_space(uint8_t &cursor_pos, uint8_t to_pos = LCD_COLS) { 509 | while(cursor_pos < to_pos) { lcd.print(' '); cursor_pos++; } 510 | } 511 | 512 | // Re-paint display 513 | void update_display() { 514 | ///////////////////////////// 1st raw /////////////////////////////// 515 | lcd.setCursor(0,0); 516 | uint8_t pos = 0; 517 | char buf[10]; 518 | 519 | cur_page = (cur_page+50)%5; // should be in the range of available pages 520 | 521 | /////// PAGE0 //////// 522 | if(cur_page == 0) { 523 | if(is_connected) { 524 | // Battery voltage 525 | sprintf(buf, "%2d.%02dV", rt_data.battery_voltage/100, rt_data.battery_voltage%100); 526 | pos+= lcd.print(buf); 527 | 528 | // Current profile 529 | pos+= lcd.print(" P:"); 530 | pos+= lcd.print(rt_data.cur_profile+1); 531 | 532 | // Max. error 533 | pos+= lcd.print(" E:"); 534 | uint16_t err = target_error_avg.res; 535 | sprintf(buf, "%03d", err); 536 | pos+= lcd.print(buf); 537 | } else { 538 | pos+= lcd.print("CONNECTING"); 539 | lcd_print_progress(pos); 540 | } 541 | } 542 | /////// PAGE1 //////// 543 | else if(cur_page == 1) { 544 | // Serial error counter 545 | pos+= lcd.print("SE:"); 546 | pos+= lcd.print(sbgc_parser.get_parse_error_count()); 547 | 548 | // Arduino free memory 549 | pos+= lcd.print(" FM:"); 550 | pos+= lcd.print(free_memory); 551 | } 552 | /////// PAGE2 //////// 553 | else if(cur_page == 2) { 554 | // TODO 555 | pos+= lcd.print("D1:"); 556 | pos+= lcd.print(debug1); 557 | pos+= lcd.print(" D2:"); 558 | pos+= lcd.print(debug2); 559 | } 560 | /////// PAGE3 //////// 561 | else if(cur_page == 3) { 562 | pos+= lcd.print("D3:"); 563 | pos+= lcd.print(debug3); 564 | pos+= lcd.print(" D4:"); 565 | pos+= lcd.print(debug4); 566 | } 567 | /////// PAGE4 //////// 568 | else if(cur_page == 4) { 569 | pos+= lcd.print("I2C_ERR:"); 570 | pos+= lcd.print(rt_data.i2c_error_count); 571 | } 572 | 573 | lcd_fill_space(pos); 574 | 575 | /////////////////////////// 2nd raw ///////////////////////////// 576 | lcd.setCursor(0,1); 577 | pos = 0; 578 | 579 | // Currently selected adj. variable name and value 580 | adjustable_var_t &var = adj_vars[cur_adj_var_idx]; 581 | pos+= lcd.print(var.cfg.name); 582 | if(pos > ADJ_VAR_NAME_MAX_LENGTH) lcd.setCursor(ADJ_VAR_NAME_MAX_LENGTH, 1); 583 | pos+= lcd.print(":"); 584 | lcd_fill_space(pos, (ADJ_VAR_NAME_MAX_LENGTH+1)); 585 | pos+= lcd.print(var.val); 586 | 587 | lcd_fill_space(pos); 588 | } 589 | 590 | 591 | // Displays a NULL-terminated string 592 | void lcd_debug_msg(char *str, uint8_t raw=1) { 593 | lcd.setCursor(0,raw); 594 | uint8_t pos = lcd.print(str); 595 | lcd_fill_space(pos); 596 | } 597 | 598 | 599 | // Reads answer to buf[] sized 'buf_size' bytes as string. 600 | // Display answer on LCD. 601 | // Returns actualy read size. 602 | uint8_t _bt_read_answer(char *buf, uint16_t timeout_ms=500, bool debug=BLUETOOTH_DEBUG) { 603 | serial.setTimeout(timeout_ms); // wait 200 ms for each answer to be accomplished 604 | uint8_t size; 605 | 606 | #if(defined(BLUETOOTH_RN42)) 607 | size = serial.readBytesUntil('\r', buf, (BLUETOOTH_BUF_SIZE-1)); 608 | #elif(defined(BLUETOOTH_HM10)) 609 | size = serial.readBytes(buf, (BLUETOOTH_BUF_SIZE-1)); 610 | #elif(defined(BLUETOOTH_HC05)) 611 | size = serial.readBytesUntil('\r', buf, (BLUETOOTH_BUF_SIZE - 1)); 612 | #else 613 | #error "Define serial.read() for bluetooth!" 614 | #endif 615 | 616 | buf[size] = '\0'; 617 | if(size) { 618 | if(debug) lcd_debug_msg(buf); 619 | } else { 620 | if(debug) { 621 | lcd_debug_msg("NO ANSWER"); 622 | delay(1000); 623 | } 624 | } 625 | 626 | serial.setTimeout(1000); // return default value 627 | 628 | return size; 629 | } 630 | 631 | 632 | /* 633 | * Initiates wireless connection. Leave it empty for wire connection. 634 | */ 635 | void bt_master_connect() { 636 | #ifdef BLUETOOTH_CONNECTION 637 | 638 | char buf[BLUETOOTH_BUF_SIZE]; 639 | lcd_debug_msg("BLUETOOTH INIT...", 0); 640 | // set baud rate as set in the BT module 641 | serial.begin(BLUETOOTH_BAUD); 642 | delay(500); // let module to start 643 | 644 | 645 | #if(defined(BLUETOOTH_RN42)) 646 | serial.print("$$$"); // start command mode 647 | _bt_read_answer(buf); 648 | 649 | 650 | #ifdef BLUETOOTH_DO_SETUP 651 | serial.println("SM,1"); // set to master mode 652 | _bt_read_answer(buf); 653 | serial.print("SP,"); // set PIN-code 654 | serial.println(BLUETOOTH_CLIENT_PIN); 655 | _bt_read_answer(buf); 656 | lcd_debug_msg("REBOOT.."); 657 | serial.println("R,1"); // reboot to apply changes 658 | _bt_read_answer(buf); 659 | delay(1000); 660 | serial.print("$$$"); // command mode again 661 | _bt_read_answer(buf); 662 | #endif // BLUETOOTH_DO_SETUP 663 | 664 | // Connect to specified device and switch to fast data mode 665 | lcd_debug_msg("START CONNECTION.."); 666 | serial.print("CF"); 667 | serial.println(BLUETOOTH_CLIENT_MAC_ADDR); 668 | 669 | 670 | #elif(defined(BLUETOOTH_HM10)) 671 | 672 | #ifdef BLUETOOTH_DO_SETUP 673 | serial.print("AT+VERS?"); 674 | _bt_read_answer(buf); 675 | serial.print("AT+IMME1"); // set AT mode at boot 676 | _bt_read_answer(buf); 677 | serial.print("AT+ROLE1"); // set "master" mode 678 | _bt_read_answer(buf); 679 | serial.print("AT+MODE0"); // set "data transmission mode" 680 | _bt_read_answer(buf); 681 | sprintf(buf, "AT+PASS%s", BLUETOOTH_CLIENT_PIN); 682 | serial.print(buf); // set PIN-code 683 | _bt_read_answer(buf); 684 | #endif // BLUETOOTH_DO_SETUP 685 | 686 | #ifdef BLUETOOTH_CLIENT_MAC_ADDR 687 | // Connect to specified device and switch to fast data mode 688 | lcd_debug_msg("START CONNECTION.."); 689 | serial.print("AT+CON"); 690 | serial.print(BLUETOOTH_CLIENT_MAC_ADDR); 691 | _bt_read_answer(buf); 692 | #else 693 | // Search BLE module by name 694 | lcd_debug_msg("SEARCHING...", 0); 695 | serial.print("AT+SHOW1"); // set showing the name os discovered device 696 | _bt_read_answer(buf); 697 | serial.print("AT+DISC?"); 698 | 699 | uint8_t device_idx = 0; 700 | int8_t found_idx = -1; 701 | for(uint8_t i=0; i<100; i++) { // wait 20 seconds 702 | if(uint8_t size = _bt_read_answer(buf, 100, false)) { // wait answer 100 ms, skip debugging 703 | if(strstr(buf, "OK+DISCE") != NULL) break; // finished discovery 704 | 705 | if(char *name_str = strstr(buf, "OK+NAME:")) { 706 | // We found a device, display its name 707 | lcd_debug_msg(&name_str[8]); 708 | if(BLUETOOTH_DEBUG) delay(500); 709 | 710 | if(strstr(&name_str[8], BLUETOOTH_CLIENT_NAME_PATTERN) != NULL) { 711 | found_idx = device_idx; 712 | } 713 | 714 | device_idx++; 715 | } 716 | } 717 | } 718 | 719 | if(found_idx >= 0) { 720 | lcd_debug_msg("CONNECTING BT..", 0); 721 | // connect using array index 722 | sprintf(buf, "AT+CONN%d", found_idx); 723 | serial.print(buf); 724 | _bt_read_answer(buf); 725 | } else { 726 | lcd_debug_msg("NO DEVICE FOUND", 0); 727 | } 728 | 729 | 730 | #endif // BLUETOOTH_CLIENT_MAC_ADDR 731 | 732 | #elif(defined(BLUETOOTH_HC05)) 733 | 734 | #ifdef BLUETOOTH_DO_SETUP 735 | 736 | serial.println("AT+RMAAD"); //Clear paired devices 737 | _bt_read_answer(buf); 738 | sprintf(buf, "AT+PSWD=%s", BLUETOOTH_CLIENT_PIN); // set PIN (both master and slave should have the same PIN) 739 | serial.println(buf); 740 | _bt_read_answer(buf); 741 | serial.println("AT+ROLE=1"); //set 'master' mode 742 | _bt_read_answer(buf); 743 | serial.println("AT+CMODE=0"); //connect only to fixed MAC address 744 | _bt_read_answer(buf); 745 | serial.println("AT+INIT"); //Initialize 'SPP' 746 | _bt_read_answer(buf); 747 | sprintf(buf, "AT+LINK=%s", BLUETOOTH_CLIENT_MAC_ADDR); // connect to slave HC05, MAC address should be in following format: "XXXX,XX,XXXXXX" ("98D3,34,90DB5E") 748 | serial.println(buf); 749 | _bt_read_answer(buf); 750 | 751 | #endif // BLUETOOTH_DO_SETUP 752 | 753 | #endif // ..BLUETOOTH TYPE.. 754 | 755 | delay(3000); 756 | 757 | //last_bt_connect_ms = millis(); 758 | 759 | 760 | #endif // BLUETOOTH_CONNECTION 761 | } 762 | -------------------------------------------------------------------------------- /LCDRemote/MemoryFree.cpp: -------------------------------------------------------------------------------- 1 | #if (ARDUINO >= 100) 2 | #include 3 | #else 4 | #include 5 | #endif 6 | 7 | extern unsigned int __heap_start; 8 | extern void *__brkval; 9 | 10 | /* 11 | * The free list structure as maintained by the 12 | * avr-libc memory allocation routines. 13 | */ 14 | struct __freelist 15 | { 16 | size_t sz; 17 | struct __freelist *nx; 18 | }; 19 | 20 | /* The head of the free list structure */ 21 | extern struct __freelist *__flp; 22 | 23 | #include "MemoryFree.h" 24 | 25 | /* Calculates the size of the free list */ 26 | int freeListSize() 27 | { 28 | struct __freelist* current; 29 | int total = 0; 30 | for (current = __flp; current; current = current->nx) 31 | { 32 | total += 2; /* Add two bytes for the memory block's header */ 33 | total += (int) current->sz; 34 | } 35 | 36 | return total; 37 | } 38 | 39 | int freeMemory() 40 | { 41 | int free_memory; 42 | if ((int)__brkval == 0) 43 | { 44 | free_memory = ((int)&free_memory) - ((int)&__heap_start); 45 | } 46 | else 47 | { 48 | free_memory = ((int)&free_memory) - ((int)__brkval); 49 | free_memory += freeListSize(); 50 | } 51 | return free_memory; 52 | } 53 | -------------------------------------------------------------------------------- /LCDRemote/MemoryFree.h: -------------------------------------------------------------------------------- 1 | // MemoryFree library based on code posted here: 2 | // http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1213583720/15 3 | // Extended by Matthew Murdoch to include walking of the free list. 4 | 5 | #ifndef MEMORY_FREE_H 6 | #define MEMORY_FREE_H 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | int freeMemory(); 13 | 14 | #ifdef __cplusplus 15 | } 16 | #endif 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /LCDRemote/filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | This is a part of examples of usage of SimpleBGC Serial API. 3 | API specs are available at http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2014 Aleksei Moskalenko 6 | */ 7 | 8 | #ifndef FILTER_H_ 9 | #define FILTER_H_ 10 | 11 | #include 12 | 13 | 14 | 15 | typedef struct { 16 | adjustable_var_cfg_t cfg; 17 | int16_t step; 18 | int32_t val; 19 | uint8_t need_update; 20 | } adjustable_var_t; 21 | 22 | 23 | typedef struct { 24 | uint16_t last_time_ms; 25 | uint8_t state; // current state 26 | uint8_t trigger_state; // de-bounced state 27 | } btn_state_t; 28 | 29 | 30 | uint8_t debounce_button(btn_state_t &btn, uint8_t new_state); 31 | 32 | 33 | class avg_var16 { 34 | int32_t buf; // internal buffer to store non-rounded average value 35 | uint8_t factor; 36 | 37 | public: 38 | int16_t res; // result (rounded to int) 39 | 40 | void init(uint8_t _factor) { 41 | factor = _factor; 42 | buf = 0; 43 | res = 0; 44 | } 45 | 46 | inline void average(int16_t data) { 47 | buf+= (int32_t)data - (int32_t)res; 48 | res = (int16_t)(buf >> factor); 49 | } 50 | }; 51 | 52 | 53 | 54 | #endif /* FILTER_H_ */ -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Aleksei Moskalenko 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 18 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 20 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 21 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 23 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | 25 | -------------------------------------------------------------------------------- /MimicControl/MimicControl.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This is example sketch for Arduino. 3 | Shows how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Demo: gimbal will repeat motion of the remote controller build of 2 potentiometers and 2 push-buttons: 7 | "Menu" button for gimbal controller 8 | "Rec" button for camera control via PWM->IR converter 9 | 10 | 11 | Arduino hardware: 12 | - 2 potentimeters (or encoders with analog output), connected to the pins A1, A2 13 | (connect GND, +5V to the side outputs of potentiometers) 14 | - Toggle buttons connected to D11, D12 15 | - (Optional) PWM-to-IR camera control adapter, connected to FC_PITCH of gimbal controller 16 | - Serial link over Bluetooth, 3DR radio or wires. 17 | 18 | Gimbal settings: 19 | - RC SPEED should be set high enough to track fast movements of a controller's handle (100..150) 20 | - Acceleration limit is set to a value that gimbal can process without loosing sync in motors 21 | 22 | Copyright (c) 2016 Aleksey Moskalenko 23 | *******************************************************************************/ 24 | #include 25 | #include 26 | #include 27 | #include "filter.h" 28 | 29 | 30 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 31 | #define SERIAL_SPEED 115200 32 | 33 | // delay between CMD_CONTROL commands, ms 34 | #define CMD_CONTROL_DELAY 20 35 | 36 | #define JOY_X_ANALOG_PIN 1 // joystick X-axis input 37 | #define JOY_Y_ANALOG_PIN 2 // joystick Y-axis input 38 | #define MENU_BTN_PIN 11 // "MENU" button input 39 | #define REC_BTN_PIN 12 // "REC" button input 40 | 41 | //#define SET_SPEED 100 // uncomment to override SPEED setting in the GUI 42 | 43 | // Extreme angles in degrees, that corresponds to 0..Vcc analog input range 44 | #define PITCH_ANGLE_MIN -60 45 | #define PITCH_ANGLE_MAX 60 46 | #define YAW_ANGLE_MIN -60 47 | #define YAW_ANGLE_MAX 60 48 | 49 | // LPF filter applyed to a signal to smooth sharp movements, 0..16, 0 - no filtering, 16 - max filtering 50 | #define LOW_PASS_FACTOR 6 51 | 52 | // PWM output port index 53 | #define PWM_SERVO_OUT_IDX 1 // 0=FC_ROLL, 1=FC_PITCH, 2=RC_PITCH, 3=AUX1 54 | 55 | // PWM values for external infra-red camera control adapter 56 | #define PWM_CAM_REC_ON 2000 57 | #define PWM_CAM_REC_OFF 1000 58 | 59 | /*****************************************************************************/ 60 | 61 | 62 | #define BTN_STATE_RELEASED 0 63 | #define BTN_STATE_PRESSED 1 64 | 65 | // Set serial port where SBGC32 is connected 66 | #define serial Serial 67 | 68 | static SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 }; 69 | static SBGC_cmd_servo_out_t cmd_servo_out = { { SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED } }; 70 | static btn_state_t menu_btn, rec_btn; 71 | static uint16_t last_cmd_control_time_ms = 0; 72 | static avg_var16 lpf[2]; 73 | 74 | 75 | 76 | void setup() { 77 | serial.begin(SERIAL_SPEED); 78 | SBGC_Demo_setup(&serial); 79 | 80 | c.mode = SBGC_CONTROL_MODE_ANGLE; 81 | #ifdef SET_SPEED 82 | c.speedPITCH = SET_SPEED; 83 | c.speedYAW = SET_SPEED; 84 | #endif 85 | 86 | lpf[0].init(LOW_PASS_FACTOR); 87 | lpf[1].init(LOW_PASS_FACTOR); 88 | 89 | pinMode(MENU_BTN_PIN, INPUT); 90 | pinMode(REC_BTN_PIN, INPUT); 91 | // set button pins pulled up to VCC, button will short it to GND (i,e, pressed state = LOW) 92 | digitalWrite(MENU_BTN_PIN, 1); 93 | digitalWrite(REC_BTN_PIN, 1); 94 | 95 | // Take a pause to let gimbal controller to initialize 96 | delay(1000); 97 | 98 | // Set servo output to "REC OFF" state 99 | cmd_servo_out.servo[PWM_SERVO_OUT_IDX] = PWM_CAM_REC_OFF; 100 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 101 | } 102 | 103 | 104 | 105 | 106 | void loop() { 107 | uint16_t cur_time_ms = millis(); 108 | 109 | // Read analog input and convert to SBGC angle range 110 | lpf[0].average( (int16_t)SBGC_DEGREE_TO_ANGLE_INT(PITCH_ANGLE_MIN) + 111 | (int16_t)((int32_t)analogRead(JOY_Y_ANALOG_PIN)*SBGC_DEGREE_TO_ANGLE_INT(PITCH_ANGLE_MAX - PITCH_ANGLE_MIN)/1024) ); 112 | lpf[1].average( (int16_t)SBGC_DEGREE_TO_ANGLE_INT(YAW_ANGLE_MIN) + 113 | (int16_t)((int32_t)analogRead(JOY_X_ANALOG_PIN)*SBGC_DEGREE_TO_ANGLE_INT(YAW_ANGLE_MAX - YAW_ANGLE_MIN)/1024) ); 114 | 115 | 116 | if((cur_time_ms - last_cmd_control_time_ms ) > CMD_CONTROL_DELAY) { 117 | last_cmd_control_time_ms = cur_time_ms; 118 | 119 | c.anglePITCH = lpf[0].res; 120 | c.angleYAW = lpf[1].res; 121 | 122 | SBGC_cmd_control_send(c, sbgc_parser); 123 | } 124 | 125 | 126 | // Process menu button and send it as a "menu" cmd press to the board 127 | if(debounce_button(menu_btn, digitalRead(MENU_BTN_PIN) == LOW ? BTN_STATE_PRESSED : BTN_STATE_RELEASED)) { 128 | if(menu_btn.trigger_state == BTN_STATE_PRESSED) { 129 | SBGC_cmd_execute_menu_send(SBGC_MENU_BUTTON_PRESS, sbgc_parser); 130 | } 131 | } 132 | 133 | // Process "REC" button and send it as a "servo out" command 134 | if(debounce_button(rec_btn, digitalRead(REC_BTN_PIN) == LOW ? BTN_STATE_PRESSED : BTN_STATE_RELEASED)) { 135 | if(rec_btn.trigger_state == BTN_STATE_PRESSED) { 136 | cmd_servo_out.servo[PWM_SERVO_OUT_IDX] = PWM_CAM_REC_ON; 137 | } else { 138 | cmd_servo_out.servo[PWM_SERVO_OUT_IDX] = PWM_CAM_REC_OFF; 139 | } 140 | 141 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 142 | } 143 | 144 | // Make a constant sampling time by inserting delay 1ms 145 | while((millis() - cur_time_ms) < 1); 146 | } 147 | 148 | 149 | 150 | /* 151 | * De-bounce button: it should keep its state for a given period of time (30ms) 152 | * Returns 1 if btn.trigger_state is changed. 153 | */ 154 | uint8_t debounce_button(btn_state_t &btn, uint8_t new_state) { 155 | uint16_t cur_time_ms = millis(); 156 | 157 | if(new_state != btn.state) { 158 | btn.state = new_state; 159 | btn.last_time_ms = cur_time_ms; 160 | } else if(btn.trigger_state != btn.state && (uint16_t)(cur_time_ms - btn.last_time_ms) > 30) { 161 | btn.trigger_state = btn.state; 162 | return 1; 163 | } 164 | return 0; 165 | } 166 | -------------------------------------------------------------------------------- /MimicControl/MimicControl.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/basecamelectronics/sbgc-api-examples/e23402d1e65c2be339590e607a7836d57f7126e2/MimicControl/MimicControl.pdf -------------------------------------------------------------------------------- /MimicControl/filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | This is a part of examples of usage of SimpleBGC Serial API. 3 | API specs are available at http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2016 Aleksei Moskalenko 6 | */ 7 | 8 | #ifndef FILTER_H_ 9 | #define FILTER_H_ 10 | 11 | #include 12 | 13 | typedef struct { 14 | uint16_t last_time_ms; 15 | uint8_t state; // current state 16 | uint8_t trigger_state; // de-bounced state 17 | } btn_state_t; 18 | 19 | uint8_t debounce_button(btn_state_t &btn, uint8_t new_state); 20 | 21 | 22 | /* Exponential moving average filter (optimized for integers) with factor = 2^n 23 | * It correctly handles averaging of 16bit angles with overflow 24 | * */ 25 | class avg_var16 { 26 | int32_t buf; // internal buffer to store non-rounded average value 27 | uint8_t factor; 28 | 29 | public: 30 | int16_t res; // result (rounded to int) 31 | 32 | void init(uint8_t _factor) { 33 | factor = _factor; 34 | buf = 0; 35 | res = 0; 36 | } 37 | 38 | inline void average(int16_t data) { 39 | buf+= (int32_t)data - (int32_t)res; 40 | res = (int16_t)(buf >> factor); 41 | } 42 | }; 43 | 44 | 45 | 46 | #endif /* FILTER_H_ */ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sbgc-api-examples 2 | SimpleBGC Serial API examples and libraries 3 | 4 | This folders contains SBGC library and example sketches for Arduino, 5 | that shows how to control SimpleBGC-driven gimbal via Serial API. 6 | 7 | API specs are available at the http://www.basecamelectronics.com/serialapi/ 8 | 9 | 10 | Contents: 11 | 12 | libraries/SBGC_lib - common library that can be used in any C/C++ project 13 | libraries/SBGC_Arduino - Arduino-specific library used in the examples 14 | 15 | 16 | How to run examples: 17 | 18 | 1. Copy the content of this folder (preserving subfolder structure) to your "Sketchbook" folder, 19 | as specified in the "File -> Preferences -> Sketchbook location" in the Arduino IDE. 20 | This step is important to let a compiler to find and include libraries. 21 | 2. Open any *.ino example in the Arduino IDE, compile and upload to your Arduino board. 22 | 3. Connect Arduino's serial interface to the UART connector on the SimpleBGC board: 23 | Arduino GND -> SimpleBGC GND 24 | Arduino TX -> SimpleBGC RX (optional) 25 | Arduino RX -> SimpleBGC TX 26 | You can power Arduino separatly or via +5V from onboard UART connector 27 | 28 | 29 | All examples were tested with the Arduino 1.0 IDE, Pro Mini and UNO boards. -------------------------------------------------------------------------------- /SerialControl/SerialControl.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This is example sketch for Arduino. 3 | Shows how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Demo: control camera angles by Serial API direct control and by 7 | emulating various RC input methods; 8 | 9 | Arduino hardware: 10 | - analog joystick on the pins A1, A2 (connect GND, +5V to the side outputs of its potentiometers) 11 | 12 | Gimbal settings: 13 | - RC control in SPEED mode, RC signal should come from active RC source 14 | - RC SPEED is set to about 30..100 15 | 16 | Copyright (c) 2014-2015 Aleksey Moskalenko 17 | *******************************************************************************/ 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 24 | #define SERIAL_SPEED 115200 25 | 26 | // delay between commands, ms 27 | #define SBGC_CMD_DELAY 20 28 | 29 | /*****************************************************************************/ 30 | 31 | // Set serial port where SBGC32 is connected 32 | #define serial Serial 33 | 34 | void setup() { 35 | serial.begin(SERIAL_SPEED); 36 | SBGC_Demo_setup(&serial); 37 | 38 | // Take a pause to let gimbal controller to initialize 39 | delay(3000); 40 | } 41 | 42 | 43 | void loop() { 44 | SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 }; 45 | 46 | 47 | // Move camera to initial position (all angles are zero) 48 | // Set speed 30 degree/sec 49 | c.mode = SBGC_CONTROL_MODE_ANGLE; 50 | c.speedROLL = c.speedPITCH = c.speedYAW = 30 * SBGC_SPEED_SCALE; 51 | SBGC_cmd_control_send(c, sbgc_parser); 52 | delay(3000); 53 | 54 | 55 | 56 | blink_led(1); 57 | /////////////////// Demo 1. PITCH and YAW gimbal by 40 and 30 degrees both sides and return back. 58 | // Actual speed depends on PID setting. 59 | // Whait 5 sec to finish 60 | c.mode = SBGC_CONTROL_MODE_ANGLE; 61 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(40); 62 | c.angleYAW = SBGC_DEGREE_TO_ANGLE(30); 63 | SBGC_cmd_control_send(c, sbgc_parser); 64 | delay(4000); 65 | 66 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(-40); 67 | c.angleYAW = SBGC_DEGREE_TO_ANGLE(-30); 68 | SBGC_cmd_control_send(c, sbgc_parser); 69 | delay(8000); 70 | 71 | // .. and back 72 | c.anglePITCH = 0; 73 | c.angleYAW = 0; 74 | SBGC_cmd_control_send(c, sbgc_parser); 75 | delay(4000); 76 | 77 | 78 | blink_led(2); 79 | /////////////////// Demo 2. Pitch gimbal down with constant speed 10 degree/sec 80 | // by 50 degree (it takes 5 sec) 81 | // (this is simplified version of speed control. To prevent jerks, you should 82 | // add acceleration and de-acceleration phase) 83 | c.mode = SBGC_CONTROL_MODE_SPEED; 84 | c.speedPITCH = 10 * SBGC_SPEED_SCALE; 85 | c.speedROLL = 0; 86 | c.speedYAW = 0; 87 | SBGC_cmd_control_send(c, sbgc_parser); 88 | delay(5000); 89 | 90 | // Stop 91 | c.speedPITCH = 0; 92 | SBGC_cmd_control_send(c, sbgc_parser); 93 | delay(1000); 94 | 95 | // .. and back 96 | c.speedPITCH = -10 * SBGC_SPEED_SCALE; 97 | SBGC_cmd_control_send(c, sbgc_parser); 98 | delay(5000); 99 | 100 | // Stop 101 | c.speedPITCH = 0; 102 | SBGC_cmd_control_send(c, sbgc_parser); 103 | delay(1000); 104 | 105 | 106 | blink_led(3); 107 | ///////////// Demo3: Return control back to RC for 5 seconds 108 | c.mode = SBGC_CONTROL_MODE_NO; 109 | SBGC_cmd_control_send(c, sbgc_parser); 110 | delay(5000); 111 | 112 | 113 | 114 | blink_led(4); 115 | //////////// Demo 4. More complicated example: Pitch gimbal by 40 degrees 116 | // with the full control of speed and angle. 117 | // - send control command with the fixed frame rate 118 | // - angle is calculated by the integration of the speed 119 | float speed = 0, angle = 0; 120 | c.mode = SBGC_CONTROL_MODE_SPEED_ANGLE; 121 | // acceleration phase 122 | while(angle < 20.0f) { 123 | speed+= 0.5f; 124 | c.speedPITCH = speed * SBGC_SPEED_SCALE; 125 | angle+= speed * SBGC_CMD_DELAY / 1000.0f; // degree/sec -> degree/ms 126 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle); 127 | SBGC_cmd_control_send(c, sbgc_parser); 128 | delay(SBGC_CMD_DELAY); 129 | } 130 | // de-acceleration phase 131 | while(angle < 40.0f && speed > 0.0f) { 132 | speed-= 0.5f; 133 | c.speedPITCH = speed * SBGC_SPEED_SCALE; 134 | angle+= speed * SBGC_CMD_DELAY / 1000.0f; 135 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle); 136 | SBGC_cmd_control_send(c, sbgc_parser); 137 | delay(SBGC_CMD_DELAY); 138 | } 139 | 140 | 141 | 142 | blink_led(5); 143 | /////////////// Demo5: Emulate RC stick input on PITCH, reading it from analog input where joystick is connected 144 | c.mode = SBGC_CONTROL_MODE_RC; 145 | c.angleROLL = c.angleYAW = 0; 146 | c.anglePITCH = analogRead(1) - (1024/2); // neutral point is 0 147 | SBGC_cmd_control_send(c, sbgc_parser); 148 | delay(5000); 149 | 150 | // Turn off serial control 151 | c.mode = SBGC_CONTROL_MODE_NO; 152 | SBGC_cmd_control_send(c, sbgc_parser); 153 | 154 | 155 | 156 | blink_led(6); 157 | //////////// Demo6: Read A1, A2 where analog joystick is connected, 158 | // and send their values to the API Virtual Channels 1 and 2. 159 | // You can map this channels to any function you like, in the GUI 160 | // (firmware v.2.43 (32bit) or above is required) 161 | SBGC_cmd_api_virt_ch_control_t v; 162 | // Mark all channels as 'no signal' 163 | for(uint8_t i=0; i 24 | #include 25 | #include 26 | 27 | 28 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 29 | #define SERIAL_SPEED 115200 30 | 31 | // Set serial port where SBGC32 is connected 32 | #define serial Serial 33 | 34 | 35 | // All servo outputs are disabled (not occupied and floating) 36 | SBGC_cmd_servo_out_t cmd_servo_out = { { SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED, SBGC_SERVO_OUT_DISABLED } }; 37 | 38 | 39 | void setup() { 40 | serial.begin(SERIAL_SPEED); 41 | SBGC_Demo_setup(&serial); 42 | 43 | // Take a pause to let gimbal controller to initialize 44 | delay(3000); 45 | 46 | // Use servo1 output (labeled as FC_ROLL) and set it to low level (no PWM generation) 47 | cmd_servo_out.servo[0] = 0; 48 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 49 | delay(1000); 50 | } 51 | 52 | 53 | void loop() { 54 | 55 | blink_led(1); 56 | /////////// Demo1: control servo connected to FC_ROLL 57 | // Move servo1 to neutral position 58 | cmd_servo_out.servo[0] = 1500; 59 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 60 | delay(1000); 61 | 62 | 63 | // Slowly move servo1 from neutral position to top 64 | for(cmd_servo_out.servo[0]=1500; cmd_servo_out.servo[0]<2500; cmd_servo_out.servo[0]+= 5) { 65 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 66 | delay(20); 67 | } 68 | 69 | // Slowly move servo1 from top to bottom 70 | for(cmd_servo_out.servo[0]=2500; cmd_servo_out.servo[0]>500; cmd_servo_out.servo[0]-= 5) { 71 | SBGC_cmd_servo_out_send(cmd_servo_out, sbgc_parser); 72 | delay(20); 73 | } 74 | delay(1000); 75 | 76 | 77 | 78 | blink_led(2); 79 | ////////// Demo2: Control the state of AUX pins and BUZZER port 80 | SBGC_cmd_trigger_t t = { 0, 0 }; 81 | t.pin = SBGC_PIN_AUX1; 82 | t.state = 1; 83 | SBGC_cmd_trigger_send(t, sbgc_parser); 84 | t.pin = SBGC_PIN_BUZZER; 85 | t.state = 1; 86 | SBGC_cmd_trigger_send(t, sbgc_parser); 87 | delay(500); 88 | t.pin = SBGC_PIN_AUX1; 89 | t.state = 0; 90 | SBGC_cmd_trigger_send(t, sbgc_parser); 91 | t.pin = SBGC_PIN_BUZZER; 92 | t.state = 0; 93 | SBGC_cmd_trigger_send(t, sbgc_parser); 94 | delay(1000); 95 | } 96 | -------------------------------------------------------------------------------- /SpeedAngleControl/SpeedAngleControl.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This is example sketch for Arduino. 3 | Shows how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Demo: control camera angles by Serial API direct control and by 7 | emulating various RC input methods; 8 | 9 | Arduino hardware: 10 | - analog joystick on the pins A1, A2 (connect GND, +5V to the side outputs of its potentiometers) 11 | 12 | Gimbal settings: 13 | - RC control in SPEED mode, RC signal should come from active RC source 14 | - RC SPEED is set to about 30..100 15 | 16 | Copyright (c) 2014-2015 Aleksey Moskalenko 17 | *******************************************************************************/ 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | // Serial baud rate should match with the rate, configured for the SimpleBGC controller 24 | #define SERIAL_SPEED 115200 25 | 26 | // delay between commands, ms 27 | #define SBGC_CMD_DELAY 10 28 | 29 | /*****************************************************************************/ 30 | 31 | // Set serial port where SBGC32 is connected 32 | #define serial Serial 33 | 34 | void setup() { 35 | serial.begin(SERIAL_SPEED); 36 | SBGC_Demo_setup(&serial); 37 | 38 | // Take a pause to let gimbal controller to initialize 39 | //delay(1000); 40 | } 41 | 42 | 43 | void loop() { 44 | SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 }; 45 | 46 | 47 | // Move camera to initial position (all angles are zero) 48 | // Set speed 30 degree/sec 49 | c.mode = SBGC_CONTROL_MODE_ANGLE; 50 | c.speedROLL = c.speedPITCH = c.speedYAW = 100 * SBGC_SPEED_SCALE; 51 | SBGC_cmd_control_send(c, sbgc_parser); 52 | delay(1000); 53 | 54 | 55 | /* 56 | blink_led(1); 57 | /////////////////// Demo 1. PITCH and YAW gimbal by 40 and 30 degrees both sides and return back. 58 | // Actual speed depends on PID setting. 59 | // Whait 5 sec to finish 60 | c.mode = SBGC_CONTROL_MODE_ANGLE; 61 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(60); 62 | SBGC_cmd_control_send(c, sbgc_parser); 63 | delay(2000); 64 | 65 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(-60); 66 | SBGC_cmd_control_send(c, sbgc_parser); 67 | delay(3000); 68 | 69 | // .. and back 70 | c.anglePITCH = 0; 71 | SBGC_cmd_control_send(c, sbgc_parser); 72 | delay(1000); 73 | 74 | 75 | blink_led(2); 76 | /////////////////// Demo 2. Pitch gimbal down with constant speed 10 degree/sec 77 | // by 50 degree (it takes 5 sec) 78 | // (this is simplified version of speed control. To prevent jerks, you should 79 | // add acceleration and de-acceleration phase) 80 | c.mode = SBGC_CONTROL_MODE_SPEED; 81 | c.speedPITCH = 40 * SBGC_SPEED_SCALE; 82 | c.speedROLL = 0; 83 | c.speedYAW = 0; 84 | SBGC_cmd_control_send(c, sbgc_parser); 85 | delay(2000); 86 | 87 | // Stop 88 | c.speedPITCH = 0; 89 | SBGC_cmd_control_send(c, sbgc_parser); 90 | delay(100); 91 | 92 | // .. and back 93 | c.speedPITCH = -40 * SBGC_SPEED_SCALE; 94 | SBGC_cmd_control_send(c, sbgc_parser); 95 | delay(4000); 96 | 97 | // Stop 98 | c.speedPITCH = 0; 99 | SBGC_cmd_control_send(c, sbgc_parser); 100 | delay(100); 101 | 102 | */ 103 | 104 | blink_led(3); 105 | //////////// Demo 4. More complicated example: Pitch gimbal by 40 degrees 106 | // with the full control of speed and angle. 107 | // - send control command with the fixed frame rate 108 | // - angle is calculated by the integration of the speed 109 | float speed = 0, angle = 0; 110 | c.mode = SBGC_CONTROL_MODE_SPEED_ANGLE; 111 | c.speedROLL = 0; 112 | c.speedYAW = 0; 113 | 114 | // acceleration phase 115 | while(angle < 40.0f) { 116 | speed+= 1.0f; 117 | c.speedPITCH = speed * SBGC_SPEED_SCALE; 118 | angle+= speed * SBGC_CMD_DELAY / 1000.0f; // degree/sec -> degree/ms 119 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle); 120 | SBGC_cmd_control_send(c, sbgc_parser); 121 | delay(SBGC_CMD_DELAY); 122 | } 123 | // de-acceleration phase 124 | while(angle < 80.0f && speed > 0.0f) { 125 | speed-= 1.0f; 126 | c.speedPITCH = speed * SBGC_SPEED_SCALE; 127 | angle+= speed * SBGC_CMD_DELAY / 1000.0f; 128 | c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle); 129 | SBGC_cmd_control_send(c, sbgc_parser); 130 | delay(SBGC_CMD_DELAY); 131 | } 132 | 133 | delay(1000); 134 | } 135 | -------------------------------------------------------------------------------- /libraries/SBGC_Arduino/SBGC_Arduino.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | This library is used by Arduino examples that show 3 | how to control SimpleBGC-driven gimbal via Serial API. 4 | API specs are available at http://www.basecamelectronics.com/serialapi/ 5 | 6 | Copyright (c) 2014-2015 Aleksey Moskalenko 7 | *******************************************************************************/ 8 | #include "Arduino.h" 9 | #include "SBGC_Arduino.h" 10 | 11 | 12 | /* Defines serial port routines required for SBGC_parser, here */ 13 | class ArduinoComObj : public SBGC_ComObj { 14 | Stream *serial; 15 | public: 16 | inline void init(Stream *s) { 17 | serial = s; 18 | } 19 | 20 | virtual uint16_t getBytesAvailable() { 21 | return serial->available(); 22 | } 23 | 24 | virtual uint8_t readByte() { 25 | return serial->read(); 26 | } 27 | 28 | virtual void writeByte(uint8_t b) { 29 | serial->write(b); 30 | } 31 | 32 | // Arduino com port is not buffered, so empty space is unknown. 33 | virtual uint16_t getOutEmptySpace() { 34 | return 0xFFFF; 35 | } 36 | 37 | }; 38 | 39 | 40 | /* Global variables */ 41 | SBGC_Parser sbgc_parser; // SBGC command parser. Define one for each port. 42 | ArduinoComObj com_obj; // COM-port wrapper required for parser 43 | 44 | 45 | 46 | // Prepare hardware, used in examples 47 | void SBGC_Demo_setup(Stream *serial) { 48 | com_obj.init(serial); 49 | sbgc_parser.init(&com_obj); 50 | 51 | // Use LED to show steps of program run 52 | pinMode(13, OUTPUT); 53 | LED_ON(); 54 | } 55 | 56 | 57 | 58 | void blink_led(uint8_t cnt) { 59 | for(uint8_t i=0; i 19 | #include "Arduino.h" 20 | #include 21 | 22 | 23 | #define LED_PIN 13 24 | 25 | extern SBGC_Parser sbgc_parser; 26 | 27 | void SBGC_Demo_setup(Stream *serial); 28 | inline void LED_ON() { digitalWrite(LED_PIN, HIGH); } 29 | inline void LED_OFF() { digitalWrite(LED_PIN, LOW); } 30 | void blink_led(uint8_t cnt); 31 | 32 | 33 | 34 | 35 | 36 | #endif -------------------------------------------------------------------------------- /libraries/SBGC_lib/SBGC.h: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | * compatible with the revision 2.4, 2.5 of Serial API specification. 6 | 7 | Copyright (c) 2014-2015 Aleksei Moskalenko 8 | All rights reserved. 9 | 10 | 11 | This software is free for use and redistribution under a BSD license: 12 | 13 | Redistribution and use in source and binary forms, with or without 14 | modification, are permitted provided that the following conditions are met: 15 | * Redistributions of source code must retain the above copyright 16 | notice, this list of conditions and the following disclaimer. 17 | * Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 20 | * Neither the name of the Basecamelectronics company nor the 21 | names of its contributors may be used to endorse or promote products 22 | derived from this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 25 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 26 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 28 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 29 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 31 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 32 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 33 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef __SBGC__ 37 | #define __SBGC__ 38 | 39 | // !!!!Define the endianess of your machine!!!! 40 | #define SYS_LITTLE_ENDIAN 41 | 42 | // !!!!Uncomment only if structures are packed (use it for 8bit machines to save some program space) 43 | //#define SYS_STRUCT_PACKED 44 | 45 | 46 | 47 | // Optimize commapnd packing if it's good aligned in the memory 48 | #if(defined(SYS_LITTLE_ENDIAN) && defined(SYS_STRUCT_PACKED)) 49 | #define SBGC_CMD_STRUCT_ALIGNED 50 | #endif 51 | 52 | 53 | 54 | #include 55 | #include "include/SBGC_command.h" 56 | #include "include/SBGC_rc.h" 57 | #include "include/SBGC_adj_vars.h" 58 | #include "include/SBGC_parser.h" 59 | #include "include/SBGC_cmd_helpers.h" 60 | 61 | 62 | 63 | // Error codes in command response 64 | #define SBGC_ERR_CMD_SIZE 1 65 | #define SBGC_ERR_WRONG_PARAMS 2 66 | #define SBGC_ERR_GET_DEVICE_ID 3 67 | #define SBGC_ERR_CRYPTO 4 68 | #define SBGC_ERR_CALIBRATE_BAT 5 69 | #define SBGC_ERR_UNKNOWN_COMMAND 6 70 | 71 | 72 | // System error flags that controller may set 73 | #define SBGC_SYS_ERR_NO_SENSOR (1<<0) 74 | #define SBGC_SYS_ERR_CALIB_ACC (1<<1) 75 | #define SBGC_SYS_ERR_SET_POWER (1<<2) 76 | #define SBGC_SYS_ERR_CALIB_POLES (1<<3) 77 | #define SBGC_SYS_ERR_PROTECTION (1<<4) 78 | #define SBGC_SYS_ERR_SERIAL (1<<5) 79 | #define SBGC_SYS_ERR_BAT_LOW (1<<6) 80 | #define SBGC_SYS_ERR_BAT_CRITICAL (1<<7) 81 | #define SBGC_SYS_ERR_GUI_VERSION (1<<8) 82 | #define SBGC_SYS_ERR_MISS_STEPS (1<<9) 83 | #define SBGC_SYS_ERR_SYSTEM (1<<10) 84 | #define SBGC_SYS_ERR_EMERGENCY_STOP (1<<11) 85 | 86 | 87 | // Trigger pins 88 | #define SBGC_PIN_AUX1 16 89 | #define SBGC_PIN_AUX2 17 90 | #define SBGC_PIN_AUX3 18 91 | #define SBGC_PIN_BUZZER 32 92 | #define CMD_PIN_SSAT_POWER 33 // pin that control Spektrum Satellite 3.3V power line (low state enables line) 93 | 94 | // Value passed in CMD_SERVO_OUT to disable servo output 95 | #define SBGC_SERVO_OUT_DISABLED -1 96 | 97 | 98 | // Menu actions (used in the SBGC_MENU_BUTTON_PRESS command, menu button assignment, RC_CMD channel assignment) 99 | #define SBGC_MENU_PROFILE1 1 100 | #define SBGC_MENU_PROFILE2 2 101 | #define SBGC_MENU_PROFILE3 3 102 | #define SBGC_MENU_SWAP_PITCH_ROLL 4 103 | #define SBGC_MENU_SWAP_YAW_ROLL 5 104 | #define SBGC_MENU_CALIB_ACC 6 105 | #define SBGC_MENU_RESET 7 106 | #define SBGC_MENU_SET_ANGLE 8 107 | #define SBGC_MENU_CALIB_GYRO 9 108 | #define SBGC_MENU_MOTOR_TOGGLE 10 109 | #define SBGC_MENU_MOTOR_ON 11 110 | #define SBGC_MENU_MOTOR_OFF 12 111 | #define SBGC_MENU_FRAME_UPSIDE_DOWN 13 112 | #define SBGC_MENU_PROFILE4 14 113 | #define SBGC_MENU_PROFILE5 15 114 | #define SBGC_MENU_AUTO_PID 16 115 | #define SBGC_MENU_LOOK_DOWN 17 116 | #define SBGC_MENU_HOME_POSITION 18 117 | #define SBGC_MENU_RC_BIND 19 118 | #define SBGC_MENU_CALIB_GYRO_TEMP 20 119 | #define SBGC_MENU_CALIB_ACC_TEMP 21 120 | #define SBGC_MENU_BUTTON_PRESS 22 121 | #define SBGC_MENU_RUN_SCRIPT1 23 122 | #define SBGC_MENU_RUN_SCRIPT2 24 123 | #define SBGC_MENU_RUN_SCRIPT3 25 124 | #define SBGC_MENU_RUN_SCRIPT4 26 125 | #define SBGC_MENU_RUN_SCRIPT5 27 126 | #define SBGC_MENU_RUN_SCRIPT6 28 127 | #define SBGC_MENU_RUN_SCRIPT7 29 128 | #define SBGC_MENU_RUN_SCRIPT8 30 129 | #define SBGC_MENU_RUN_SCRIPT9 31 130 | #define SBGC_MENU_RUN_SCRIPT10 32 131 | #define SBGC_MENU_CALIB_MAG 33 132 | #define SBGC_MENU_LEVEL_ROLL_PITCH 34 133 | #define SBGC_MENU_CENTER_YAW 35 134 | #define SBGC_MENU_UNTWIST_CABLES 36 135 | #define SBGC_MENU_SET_ANGLE_NO_SAVE 37 136 | #define SBGC_MENU_HOME_POSITION_SHORTEST 38 137 | #define SBGC_MENU_CENTER_YAW_SHORTEST 39 138 | 139 | 140 | // Control modes 141 | #define SBGC_CONTROL_MODE_NO 0 142 | #define SBGC_CONTROL_MODE_SPEED 1 143 | #define SBGC_CONTROL_MODE_ANGLE 2 144 | #define SBGC_CONTROL_MODE_SPEED_ANGLE 3 145 | #define SBGC_CONTROL_MODE_RC 4 146 | #define SBGC_CONTROL_MODE_ANGLE_REL_FRAME 5 147 | 148 | #define SBGC_CONTROL_MODE_MASK 0x0F // bits0..3 used for mode, other for flags 149 | 150 | #define SBGC_CONTROL_MODE_FLAG_UNTWIST (1<<7) 151 | 152 | 153 | #endif //__SBGC__ -------------------------------------------------------------------------------- /libraries/SBGC_lib/SBGC_cmd_helpers.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library - helpers to pack and parse command data 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2014-2015 Aleksei Moskalenko 6 | All rights reserved. 7 | 8 | See license info in the SBGC.h 9 | */ 10 | 11 | #include 12 | #include "SBGC.h" 13 | 14 | /* Packs command structure to SerialCommand object */ 15 | void SBGC_cmd_control_pack(SBGC_cmd_control_t &p, SerialCommand &cmd) { 16 | cmd.init(SBGC_CMD_CONTROL); 17 | #ifdef SBGC_CMD_STRUCT_ALIGNED 18 | memcpy(cmd.data, &p, sizeof(p)); 19 | cmd.len = sizeof(p); 20 | #else 21 | cmd.writeByte(p.mode); 22 | cmd.writeWord(p.speedROLL); 23 | cmd.writeWord(p.angleROLL); 24 | cmd.writeWord(p.speedPITCH); 25 | cmd.writeWord(p.anglePITCH); 26 | cmd.writeWord(p.speedYAW); 27 | cmd.writeWord(p.angleYAW); 28 | #endif 29 | } 30 | 31 | 32 | /* Packs command structure to SerialCommand object */ 33 | void SBGC_cmd_control_ext_pack(SBGC_cmd_control_ext_t &p, SerialCommand &cmd) { 34 | cmd.init(SBGC_CMD_CONTROL); 35 | #ifdef SBGC_CMD_STRUCT_ALIGNED 36 | memcpy(cmd.data, &p, sizeof(p)); 37 | cmd.len = sizeof(p); 38 | #else 39 | cmd.writeBuf(p.mode, 3); 40 | for(uint8_t i=0; i<3; i++) { 41 | cmd.writeWord(p.data[i].speed); 42 | cmd.writeWord(p.data[i].angle); 43 | } 44 | #endif 45 | } 46 | 47 | 48 | 49 | 50 | 51 | /* Packs command structure to SerialCommand object */ 52 | void SBGC_cmd_api_virt_ch_control_pack(SBGC_cmd_api_virt_ch_control_t &p, SerialCommand &cmd) { 53 | cmd.init(SBGC_CMD_API_VIRT_CH_CONTROL); 54 | #ifdef SBGC_CMD_STRUCT_ALIGNED 55 | memcpy(cmd.data, &p, sizeof(p)); 56 | cmd.len = sizeof(p); 57 | #else 58 | for(uint8_t i=0; i 15 | 16 | // ID of all available variables 17 | enum { 18 | ADJ_VAR_P_ROLL = 0 19 | , ADJ_VAR_P_PITCH 20 | , ADJ_VAR_P_YAW 21 | , ADJ_VAR_I_ROLL 22 | , ADJ_VAR_I_PITCH 23 | , ADJ_VAR_I_YAW 24 | , ADJ_VAR_D_ROLL 25 | , ADJ_VAR_D_PITCH 26 | , ADJ_VAR_D_YAW 27 | , ADJ_VAR_POWER_ROLL 28 | , ADJ_VAR_POWER_PITCH 29 | , ADJ_VAR_POWER_YAW 30 | , ADJ_VAR_ACC_LIMITER 31 | , ADJ_VAR_FOLLOW_SPEED_ROLL 32 | , ADJ_VAR_FOLLOW_SPEED_PITCH 33 | , ADJ_VAR_FOLLOW_SPEED_YAW 34 | , ADJ_VAR_FOLLOW_LPF_ROLL 35 | , ADJ_VAR_FOLLOW_LPF_PITCH 36 | , ADJ_VAR_FOLLOW_LPF_YAW 37 | , ADJ_VAR_RC_SPEED_ROLL 38 | , ADJ_VAR_RC_SPEED_PITCH 39 | , ADJ_VAR_RC_SPEED_YAW 40 | , ADJ_VAR_RC_LPF_ROLL 41 | , ADJ_VAR_RC_LPF_PITCH 42 | , ADJ_VAR_RC_LPF_YAW 43 | , ADJ_VAR_RC_TRIM_ROLL 44 | , ADJ_VAR_RC_TRIM_PITCH 45 | , ADJ_VAR_RC_TRIM_YAW 46 | , ADJ_VAR_RC_DEADBAND 47 | , ADJ_VAR_RC_EXPO_RATE 48 | , ADJ_VAR_FOLLOW_MODE 49 | , ADJ_VAR_RC_FOLLOW_YAW 50 | , ADJ_VAR_FOLLOW_DEADBAND 51 | , ADJ_VAR_FOLLOW_EXPO_RATE 52 | , ADJ_VAR_FOLLOW_ROLL_MIX_START 53 | , ADJ_VAR_FOLLOW_ROLL_MIX_RANGE 54 | , ADJ_VAR_GYRO_TRUST 55 | , ADJ_VAR_FRAME_HEADING 56 | , ADJ_VAR_GYRO_HEADING_CORR 57 | , ADJ_VAL_ACC_LIMITER_ROLL 58 | , ADJ_VAL_ACC_LIMITER_PITCH 59 | , ADJ_VAL_ACC_LIMITER_YAW 60 | }; 61 | 62 | 63 | #define ADJ_VAR_NAME_MAX_LENGTH 10 64 | 65 | // Descriptors for adjustable variables 66 | typedef struct { 67 | uint8_t id; 68 | char *name; // max. 10 characters 69 | int32_t min_val; 70 | int32_t max_val; 71 | } adjustable_var_cfg_t; 72 | 73 | #define ADJ_VAR_DEF_P_ROLL { ADJ_VAR_P_ROLL, "PID_P.R", 0, 255 } 74 | #define ADJ_VAR_DEF_P_PITCH { ADJ_VAR_P_PITCH, "PID_P.P", 0, 255 } 75 | #define ADJ_VAR_DEF_P_YAW { ADJ_VAR_P_YAW, "PID_P.Y", 0, 255 } 76 | #define ADJ_VAR_DEF_I_ROLL { ADJ_VAR_I_ROLL, "PID_I.R", 0, 255 } 77 | #define ADJ_VAR_DEF_I_PITCH { ADJ_VAR_I_PITCH, "PID_I.P", 0, 255 } 78 | #define ADJ_VAR_DEF_I_YAW { ADJ_VAR_I_YAW, "PID_I.Y", 0, 255 } 79 | #define ADJ_VAR_DEF_D_ROLL { ADJ_VAR_D_ROLL, "PID_D.R", 0, 255 } 80 | #define ADJ_VAR_DEF_D_PITCH { ADJ_VAR_D_PITCH, "PID_D.P", 0, 255 } 81 | #define ADJ_VAR_DEF_D_YAW { ADJ_VAR_D_YAW, "PID_D.Y", 0, 255 } 82 | #define ADJ_VAR_DEF_POWER_ROLL { ADJ_VAR_POWER_ROLL, "POWER.R", 0, 255 } 83 | #define ADJ_VAR_DEF_POWER_PITCH { ADJ_VAR_POWER_PITCH, "POWER.P", 0, 255 } 84 | #define ADJ_VAR_DEF_POWER_YAW { ADJ_VAR_POWER_YAW, "POWER.Y", 0, 255 } 85 | #define ADJ_VAR_DEF_ACC_LIMIT { ADJ_VAR_ACC_LIMITER, "ACC_LIMIT", 0, 200} 86 | #define ADJ_VAR_DEF_FOLLOW_SPEED_ROLL { ADJ_VAR_FOLLOW_SPEED_ROLL, "F_SPD.R", 0, 255 } 87 | #define ADJ_VAR_DEF_FOLLOW_SPEED_PITCH { ADJ_VAR_FOLLOW_SPEED_PITCH, "F_SPD.P", 0, 255 } 88 | #define ADJ_VAR_DEF_FOLLOW_SPEED_YAW { ADJ_VAR_FOLLOW_SPEED_YAW, "F_SPD.Y", 0, 255 } 89 | #define ADJ_VAR_DEF_FOLLOW_LPF_ROLL { ADJ_VAR_FOLLOW_LPF_ROLL, "F_LPF.R", 0, 16 } 90 | #define ADJ_VAR_DEF_FOLLOW_LPF_PITCH { ADJ_VAR_FOLLOW_LPF_PITCH, "F_LPF.P", 0, 16 } 91 | #define ADJ_VAR_DEF_FOLLOW_LPF_YAW { ADJ_VAR_FOLLOW_LPF_YAW, "F_LPF.Y", 0, 16 } 92 | #define ADJ_VAR_DEF_RC_SPEED_ROLL { ADJ_VAR_RC_SPEED_ROLL, "RC_SPD.R", 0, 255 } 93 | #define ADJ_VAR_DEF_RC_SPEED_PITCH { ADJ_VAR_RC_SPEED_PITCH, "RC_SPD.P", 0, 255 } 94 | #define ADJ_VAR_DEF_RC_SPEED_YAW { ADJ_VAR_RC_SPEED_YAW, "RC_SPD.Y", 0, 255 } 95 | #define ADJ_VAR_DEF_RC_LPF_ROLL { ADJ_VAR_RC_LPF_ROLL, "RC_LPF.R", 0, 16 } 96 | #define ADJ_VAR_DEF_RC_LPF_PITCH { ADJ_VAR_RC_LPF_PITCH, "RC_LPF.P", 0, 16 } 97 | #define ADJ_VAR_DEF_RC_LPF_YAW { ADJ_VAR_RC_LPF_YAW, "RC_LPF.Y", 0, 16 } 98 | #define ADJ_VAR_DEF_RC_TRIM_ROLL { ADJ_VAR_RC_TRIM_ROLL, "RC_TRIM.R", -127, 127 } 99 | #define ADJ_VAR_DEF_RC_TRIM_PITCH { ADJ_VAR_RC_TRIM_PITCH, "RC_TRIM.P", -127, 127 } 100 | #define ADJ_VAR_DEF_RC_TRIM_YAW { ADJ_VAR_RC_TRIM_YAW, "RC_TRIM.Y", -127, 127 } 101 | #define ADJ_VAR_DEF_RC_DEADBAND { ADJ_VAR_RC_DEADBAND, "RC_DBND", 0, 255 } 102 | #define ADJ_VAR_DEF_RC_EXPO_RATE { ADJ_VAR_RC_EXPO_RATE, "RC_EXPO", 0, 100 } 103 | #define ADJ_VAR_DEF_FOLLOW_MODE { ADJ_VAR_FOLLOW_MODE, "F.MODE", 0, 2 } 104 | #define ADJ_VAR_DEF_FOLLOW_DEADBAND { ADJ_VAR_FOLLOW_DEADBAND, "F.DBND", 0, 255 } 105 | #define ADJ_VAR_DEF_FOLLOW_EXPO_RATE { ADJ_VAR_FOLLOW_EXPO_RATE, "F.EXPO", 0, 100 } 106 | #define ADJ_VAR_DEF_FOLLOW_ROLL_MIX_START { ADJ_VAR_FOLLOW_ROLL_MIX_START, "F.RMS", 0, 90 } 107 | #define ADJ_VAR_DEF_FOLLOW_ROLL_MIX_RANGE { ADJ_VAR_FOLLOW_ROLL_MIX_RANGE, "F.RMR", 0, 90 } 108 | #define ADJ_VAR_DEF_GYRO_TRUST { ADJ_VAR_GYRO_TRUST, "GYRO_TRUST", 1, 255} 109 | 110 | 111 | 112 | 113 | #endif // __SBGC_adj_vars__ 114 | -------------------------------------------------------------------------------- /libraries/SBGC_lib/include/SBGC_cmd_helpers.h: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library - helpers to pack and parse command data 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2014-2015 Aleksei Moskalenko 6 | All rights reserved. 7 | 8 | See license info in the SBGC.h 9 | */ 10 | 11 | #ifndef __SBGC_CMD_HELPERS__ 12 | #define __SBGC_CMD_HELPERS__ 13 | 14 | 15 | //////////////// Units conversion ///////////////// 16 | #define SBGC_ANGLE_FULL_TURN 16384 17 | // Conversion from degree/sec to units that command understand 18 | #define SBGC_SPEED_SCALE (1.0f/0.1220740379f) 19 | #define SBGC_DEGREE_ANGLE_SCALE ((float)SBGC_ANGLE_FULL_TURN/360.0f) 20 | #define SBGC_ANGLE_DEGREE_SCALE (360.0f/(float)SBGC_ANGLE_FULL_TURN) 21 | 22 | 23 | // Conversions for angle in degrees to angle in SBGC 14bit representation, and back 24 | #define SBGC_DEGREE_TO_ANGLE(val) ((val)*SBGC_DEGREE_ANGLE_SCALE) 25 | #define SBGC_ANGLE_TO_DEGREE(val) ((val)*SBGC_ANGLE_DEGREE_SCALE) 26 | // The same, optimized for integers 27 | #define SBGC_DEGREE_TO_ANGLE_INT(val) ((int32_t)(val)*SBGC_ANGLE_FULL_TURN/360) 28 | #define SBGC_DEGREE_01_TO_ANGLE_INT(val) ((int32_t)(val)*SBGC_ANGLE_FULL_TURN/3600) 29 | #define SBGC_ANGLE_TO_DEGREE_INT(val) ((int32_t)(val)*360/SBGC_ANGLE_FULL_TURN) 30 | #define SBGC_ANGLE_TO_DEGREE_01_INT(val) ((int32_t)(val)*3600/SBGC_ANGLE_FULL_TURN) 31 | 32 | 33 | 34 | #define ROLL 0 35 | #define PITCH 1 36 | #define YAW 2 37 | 38 | 39 | 40 | // CMD_CONTROL 41 | typedef struct { 42 | uint8_t mode; 43 | int16_t speedROLL; 44 | int16_t angleROLL; 45 | int16_t speedPITCH; 46 | int16_t anglePITCH; 47 | int16_t speedYAW; 48 | int16_t angleYAW; 49 | } SBGC_cmd_control_t; 50 | 51 | void SBGC_cmd_control_pack(SBGC_cmd_control_t &p, SerialCommand &cmd); 52 | inline uint8_t SBGC_cmd_control_send(SBGC_cmd_control_t &p, SBGC_Parser &parser) { 53 | SerialCommand cmd; 54 | SBGC_cmd_control_pack(p, cmd); 55 | return parser.send_cmd(cmd); 56 | } 57 | 58 | 59 | // CMD_CONTROL (extended version) 60 | typedef struct { 61 | uint8_t mode[3]; 62 | struct { 63 | int16_t angle; 64 | int16_t speed; 65 | } data[3]; 66 | } SBGC_cmd_control_ext_t; 67 | 68 | void SBGC_cmd_control_ext_pack(SBGC_cmd_control_ext_t &p, SerialCommand &cmd); 69 | inline uint8_t SBGC_cmd_control_ext_send(SBGC_cmd_control_ext_t &p, SBGC_Parser &parser) { 70 | SerialCommand cmd; 71 | SBGC_cmd_control_ext_pack(p, cmd); 72 | return parser.send_cmd(cmd); 73 | } 74 | 75 | 76 | // CMD_API_VIRT_CH_CONTROL 77 | typedef struct { 78 | int16_t data[SBGC_API_VIRT_NUM_CHANNELS]; 79 | } SBGC_cmd_api_virt_ch_control_t; 80 | 81 | void SBGC_cmd_api_virt_ch_control_pack(SBGC_cmd_api_virt_ch_control_t &p, SerialCommand &cmd); 82 | inline uint8_t SBGC_cmd_api_virt_ch_control_send(SBGC_cmd_api_virt_ch_control_t &p, SBGC_Parser &parser) { 83 | SerialCommand cmd; 84 | SBGC_cmd_api_virt_ch_control_pack(p, cmd); 85 | return parser.send_cmd(cmd); 86 | } 87 | 88 | 89 | // CMD_TRIGGER_PIN 90 | typedef struct { 91 | uint8_t pin; 92 | int8_t state; 93 | } SBGC_cmd_trigger_t; 94 | 95 | void SBGC_cmd_trigger_pack(SBGC_cmd_trigger_t &p, SerialCommand &cmd); 96 | inline uint8_t SBGC_cmd_trigger_send(SBGC_cmd_trigger_t &p, SBGC_Parser &parser) { 97 | SerialCommand cmd; 98 | SBGC_cmd_trigger_pack(p, cmd); 99 | return parser.send_cmd(cmd); 100 | } 101 | 102 | 103 | // CMD_SERVO_OUT 104 | typedef struct { 105 | int16_t servo[8]; 106 | } SBGC_cmd_servo_out_t; 107 | 108 | void SBGC_cmd_servo_out_pack(SBGC_cmd_servo_out_t &p, SerialCommand &cmd); 109 | inline uint8_t SBGC_cmd_servo_out_send(SBGC_cmd_servo_out_t &p, SBGC_Parser &parser) { 110 | SerialCommand cmd; 111 | SBGC_cmd_servo_out_pack(p, cmd); 112 | return parser.send_cmd(cmd); 113 | } 114 | 115 | 116 | //CMD_SET_ADJ_VARS_VAL 117 | typedef struct { 118 | uint8_t id; 119 | int32_t val; 120 | } SBGC_cmd_set_adj_vars_var_t; 121 | 122 | void SBGC_cmd_set_adj_vars_pack(SBGC_cmd_set_adj_vars_var_t vars[], uint8_t vars_num, SerialCommand &cmd); 123 | uint8_t SBGC_cmd_set_adj_vars_unpack(SBGC_cmd_set_adj_vars_var_t vars_buf[], uint8_t &vars_num, SerialCommand &cmd); 124 | inline uint8_t SBGC_cmd_set_adj_vars_send(SBGC_cmd_set_adj_vars_var_t vars[], uint8_t vars_num, SBGC_Parser &parser) { 125 | SerialCommand cmd; 126 | SBGC_cmd_set_adj_vars_pack(vars, vars_num, cmd); 127 | return parser.send_cmd(cmd); 128 | } 129 | 130 | 131 | 132 | // CMD_REALTIME_DATA_3, CMD_REALTIME_DATA_4 133 | typedef struct { 134 | struct { 135 | int16_t acc_data; 136 | int16_t gyro_data; 137 | } sensor_data[3]; // ACC and Gyro sensor data (with calibration) for current IMU (see cur_imu field) 138 | int16_t serial_error_cnt; // counter for communication errors 139 | int16_t system_error; // system error flags, defined in SBGC_SYS_ERR_XX 140 | uint8_t reserved1[4]; 141 | int16_t rc_raw_data[SBGC_RC_NUM_CHANNELS]; // RC signal in 1000..2000 range for ROLL, PITCH, YAW, CMD, EXT_ROLL, EXT_PITCH channels 142 | int16_t imu_angle[3]; // ROLL, PITCH, YAW Euler angles of a camera, 16384/360 degrees 143 | int16_t frame_imu_angle[3]; // ROLL, PITCH, YAW Euler angles of a frame, if known 144 | int16_t target_angle[3]; // ROLL, PITCH, YAW target angle 145 | uint16_t cycle_time_us; // cycle time in us. Normally should be 800us 146 | uint16_t i2c_error_count; // I2C errors counter 147 | uint8_t reserved2; 148 | uint16_t battery_voltage; // units 0.01 V 149 | uint8_t state_flags1; // bit0: motor ON/OFF state; bits1..7: reserved 150 | uint8_t cur_imu; // actually selecteted IMU for monitoring. 1: main IMU, 2: frame IMU 151 | uint8_t cur_profile; // active profile number starting from 0 152 | uint8_t motor_power[3]; // actual motor power for ROLL, PITCH, YAW axis, 0..255 153 | 154 | // Fields below are filled only for CMD_REALTIME_DATA_4 command 155 | int16_t rotor_angle[3]; // relative angle of each motor, 16384/360 degrees 156 | uint8_t reserved3; 157 | int16_t balance_error[3]; // error in balance. Ranges from -512 to 512, 0 means perfect balance. 158 | uint16_t current; // Current that gimbal takes, in mA. 159 | int16_t magnetometer_data[3]; // magnetometer sensor data (with calibration) 160 | int8_t imu_temp_celcius; // temperature measured by the main IMU sensor, in Celsius 161 | int8_t frame_imu_temp_celcius; // temperature measured by the frame IMU sensor, in Celsius 162 | uint8_t reserved4[38]; 163 | } SBGC_cmd_realtime_data_t; 164 | 165 | uint8_t SBGC_cmd_realtime_data_unpack(SBGC_cmd_realtime_data_t &p, SerialCommand &cmd); 166 | 167 | 168 | 169 | inline uint8_t SBGC_cmd_execute_menu_send(uint8_t menu_action, SBGC_Parser &parser) { 170 | SerialCommand cmd; 171 | cmd.init(SBGC_CMD_EXECUTE_MENU); 172 | cmd.writeByte(menu_action); 173 | return parser.send_cmd(cmd); 174 | } 175 | 176 | 177 | #endif 178 | -------------------------------------------------------------------------------- /libraries/SBGC_lib/include/SBGC_command.h: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library - definition of commands 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | 6 | Copyright (c) 2014-2015 Aleksei Moskalenko 7 | All rights reserved. 8 | 9 | See license info in the SBGC.h 10 | */ 11 | 12 | #ifndef __SBGC_command__ 13 | #define __SBGC_command__ 14 | 15 | #include 16 | #include "SBGC_rc.h" 17 | 18 | 19 | // Size of header and checksums 20 | #define SBGC_CMD_NON_PAYLOAD_BYTES 5 21 | // Max. size of a command after packing to bytes 22 | #define SBGC_CMD_MAX_BYTES 255 23 | // Max. size of a payload data 24 | #define SBGC_CMD_DATA_SIZE (SBGC_CMD_MAX_BYTES - SBGC_CMD_NON_PAYLOAD_BYTES) 25 | 26 | 27 | 28 | ////////////////////// Command ID definitions //////////////// 29 | #define SBGC_CMD_READ_PARAMS 82 30 | #define SBGC_CMD_WRITE_PARAMS 87 31 | #define SBGC_CMD_REALTIME_DATA 68 32 | #define SBGC_CMD_BOARD_INFO 86 33 | #define SBGC_CMD_CALIB_ACC 65 34 | #define SBGC_CMD_CALIB_GYRO 103 35 | #define SBGC_CMD_CALIB_EXT_GAIN 71 36 | #define SBGC_CMD_USE_DEFAULTS 70 37 | #define SBGC_CMD_CALIB_POLES 80 38 | #define SBGC_CMD_RESET 114 39 | #define SBGC_CMD_HELPER_DATA 72 40 | #define SBGC_CMD_CALIB_OFFSET 79 41 | #define SBGC_CMD_CALIB_BAT 66 42 | #define SBGC_CMD_MOTORS_ON 77 43 | #define SBGC_CMD_MOTORS_OFF 109 44 | #define SBGC_CMD_CONTROL 67 45 | #define SBGC_CMD_TRIGGER_PIN 84 46 | #define SBGC_CMD_EXECUTE_MENU 69 47 | #define SBGC_CMD_GET_ANGLES 73 48 | #define SBGC_CMD_CONFIRM 67 49 | 50 | 51 | // Starting from board ver.3.0 52 | #define SBGC_CMD_BOARD_INFO_3 20 53 | #define SBGC_CMD_READ_PARAMS_3 21 54 | #define SBGC_CMD_WRITE_PARAMS_3 22 55 | #define SBGC_CMD_REALTIME_DATA_3 23 56 | #define SBGC_CMD_SELECT_IMU_3 24 57 | #define SBGC_CMD_REALTIME_DATA_4 25 58 | #define SBGC_CMD_ENCODERS_CALIB_OFFSET_4 26 59 | #define SBGC_CMD_ENCODERS_CALIB_FLD_OFFSET_4 27 60 | #define SBGC_CMD_READ_PROFILE_NAMES 28 61 | #define SBGC_CMD_WRITE_PROFILE_NAMES 29 62 | 63 | #define SBGC_CMD_QUEUE_PARAMS_INFO_3 30 64 | #define SBGC_CMD_SET_ADJ_VARS_VAL 31 65 | #define SBGC_CMD_SAVE_PARAMS_3 32 66 | #define SBGC_CMD_READ_PARAMS_EXT 33 67 | #define SBGC_CMD_WRITE_PARAMS_EXT 34 68 | #define SBGC_CMD_AUTO_PID 35 69 | #define SBGC_CMD_SERVO_OUT 36 70 | #define SBGC_CMD_BODE_TEST_START_STOP 37 71 | #define SBGC_CMD_BODE_TEST_DATA 38 72 | #define SBGC_CMD_I2C_WRITE_REG_BUF 39 73 | #define SBGC_CMD_I2C_READ_REG_BUF 40 74 | #define SBGC_CMD_WRITE_EXTERNAL_DATA 41 75 | #define SBGC_CMD_READ_EXTERNAL_DATA 42 76 | #define SBGC_CMD_READ_ADJ_VARS_CFG 43 77 | #define SBGC_CMD_WRITE_ADJ_VARS_CFG 44 78 | #define SBGC_CMD_API_VIRT_CH_CONTROL 45 79 | #define SBGC_CMD_ADJ_VARS_STATE 46 80 | #define SBGC_CMD_EEPROM_WRITE 47 81 | #define SBGC_CMD_EEPROM_READ 48 82 | #define SBGC_CMD_CALIB_INFO 49 83 | #define SBGC_CMD_SIGN_MESSAGE_3 50 84 | #define SBGC_CMD_BOOT_MODE_3 51 85 | #define SBGC_CMD_SYSTEM_STATE 52 86 | #define SBGC_CMD_READ_FILE 53 87 | #define SBGC_CMD_WRITE_FILE 54 88 | #define SBGC_CMD_FS_CLEAR_ALL 55 89 | #define SBGC_CMD_AHRS_HELPER 56 90 | #define SBGC_CMD_RUN_SCRIPT 57 91 | #define SBGC_CMD_SCRIPT_DEBUG 58 92 | #define SBGC_CMD_CALIB_MAG 59 93 | #define SBGC_CMD_UART_BYPASS 60 94 | #define SBGC_CMD_GET_ANGLES_EXT 61 95 | #define SBGC_CMD_READ_PARAMS_EXT2 62 96 | #define SBGC_CMD_WRITE_PARAMS_EXT2 63 97 | #define SBGC_CMD_GET_ADJ_VARS_VAL 64 98 | #define SBGC_CMD_CALIB_MOTOR_MAG_LINK 74 99 | #define SBGC_CMD_GYRO_CORRECTION 75 100 | 101 | 102 | #define SBGC_CMD_DEBUG_VARS_INFO_3 253 103 | #define SBGC_CMD_DEBUG_VARS_3 254 104 | #define SBGC_CMD_ERROR 255 105 | 106 | 107 | 108 | #endif //__SBGC_command__ -------------------------------------------------------------------------------- /libraries/SBGC_lib/include/SBGC_parser.h: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library - input data parser 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2014-2015 Aleksei Moskalenko 6 | All rights reserved. 7 | 8 | See license info in the SBGC.h 9 | */ 10 | 11 | #ifndef __SBGC_parser__ 12 | #define __SBGC_parser__ 13 | 14 | #include 15 | #include 16 | #include "SBGC_command.h" 17 | 18 | 19 | #define SBGC_CMD_START_BYTE '>' 20 | 21 | typedef enum { 22 | PARSER_NO_ERROR=0, 23 | PARSER_ERROR_PROTOCOL=1, 24 | PARSER_ERROR_WRONG_CMD_SIZE=2, 25 | PARSER_ERROR_BUFFER_IS_FULL=3, 26 | PARSER_ERROR_WRONG_DATA_SIZE=4, 27 | } SBGC_parser_errors; 28 | 29 | 30 | /* 31 | * Abstract class that implements general primitive reading/writing from stream. 32 | */ 33 | class SBGC_IOStream { 34 | public: 35 | 36 | // Methods need to be implemented 37 | virtual uint16_t getBytesAvailable() = 0; 38 | virtual uint8_t readByte() = 0; 39 | virtual void writeByte(uint8_t b) = 0; 40 | 41 | 42 | int16_t readWord() { 43 | return (uint16_t)readByte() + ((uint16_t)readByte()<<8); 44 | } 45 | 46 | 47 | #ifdef SYS_LITTLE_ENDIAN 48 | // Optimization for little-endian machines only! 49 | inline void readWordArr(int16_t *arr, uint8_t size) { 50 | readBuf(arr, (size*2)); 51 | } 52 | #else 53 | void readWordArr(int16_t *arr, uint8_t size) { 54 | for(uint8_t i=0; i 0) { 78 | readByte(); 79 | } 80 | } 81 | 82 | void writeWord(int16_t w) { 83 | writeByte(w); // low 84 | writeByte(w>>8); // high 85 | } 86 | 87 | 88 | #ifdef SYS_LITTLE_ENDIAN 89 | // Optimization for little-endian machines only! 90 | inline void writeWordArr(int16_t *arr, uint8_t size) { 91 | writeBuf(arr, (size*2)); 92 | } 93 | #else 94 | void writeWordArr(int16_t *arr, uint8_t size) { 95 | for(uint8_t i=0; i>16); // high word 104 | } 105 | 106 | 107 | void writeFloat(float f) { 108 | writeBuf(&f, sizeof(float)); 109 | } 110 | 111 | void writeBuf(const void* buf, uint8_t size) { 112 | for(uint8_t i=0; i 0) { 125 | writeByte(0); 126 | } 127 | } 128 | }; 129 | 130 | 131 | /* Class to manipulate command data */ 132 | class SerialCommand : public SBGC_IOStream { 133 | public: 134 | uint8_t pos; 135 | uint8_t id; 136 | uint8_t data[SBGC_CMD_DATA_SIZE]; 137 | uint8_t len; 138 | 139 | 140 | /* Check if limit reached after reading data buffer */ 141 | inline uint8_t checkLimit() { 142 | return len == pos; 143 | } 144 | 145 | inline uint16_t getBytesAvailable() { 146 | return len - pos; 147 | } 148 | 149 | 150 | void init(uint8_t _id) { 151 | id = _id; 152 | len = 0; 153 | pos = 0; 154 | } 155 | 156 | inline void reset() { 157 | len = 0; 158 | pos = 0; 159 | } 160 | 161 | 162 | uint8_t readByte() { 163 | if(pos < len) { 164 | return data[pos++]; 165 | } else { 166 | pos++; 167 | return 0; 168 | } 169 | } 170 | 171 | 172 | void writeByte(uint8_t b) { 173 | if(len < sizeof(data)) { 174 | data[len++] = b; 175 | } 176 | } 177 | 178 | 179 | 180 | static inline void init_checksum(uint8_t &checksum) { 181 | checksum = 0; 182 | } 183 | 184 | static inline void update_checksum(uint8_t &checksum, uint8_t byte) { 185 | checksum+= byte; 186 | } 187 | }; 188 | 189 | 190 | /* Need to be implemented in the main code */ 191 | class SBGC_ComObj { 192 | public: 193 | // Return the number of bytes received in input buffer 194 | virtual uint16_t getBytesAvailable() = 0; 195 | // Read byte from the input stream 196 | virtual uint8_t readByte() = 0; 197 | // Write byte to the output stream 198 | virtual void writeByte(uint8_t b) = 0; 199 | // Return the space available in the output buffer. Return 0xFFFF if unknown. 200 | virtual uint16_t getOutEmptySpace() = 0; 201 | }; 202 | 203 | 204 | /* Optimized version of a parser, that does not require a separate buffer for maintain the parsed data */ 205 | class SBGC_Parser { 206 | SBGC_ComObj *com_obj; 207 | enum {STATE_WAIT, STATE_GOT_MARKER, STATE_GOT_ID, STATE_GOT_LEN, STATE_GOT_HEADER, STATE_GOT_DATA } state; 208 | uint16_t len; 209 | uint8_t checksum; 210 | uint16_t parser_error_count; 211 | 212 | 213 | public: 214 | SerialCommand in_cmd; // received command is stored here 215 | 216 | inline void init(SBGC_ComObj *_com_obj) { 217 | com_obj = _com_obj; 218 | state = STATE_WAIT; 219 | parser_error_count = 0; 220 | } 221 | 222 | inline void onParseError(uint8_t error = PARSER_ERROR_PROTOCOL) { 223 | parser_error_count++; 224 | } 225 | 226 | /* 227 | * Parses next character in a stream. 228 | * Returns 1 if command successfully parsed, 0 otherwise. 229 | * Calls onParseError() in case of errors 230 | */ 231 | inline uint8_t process_char(uint8_t c) { 232 | switch(state) { 233 | case STATE_WAIT: 234 | if(c == SBGC_CMD_START_BYTE) { 235 | state = STATE_GOT_MARKER; 236 | } else { 237 | //onParseError(); 238 | } 239 | break; 240 | 241 | case STATE_GOT_MARKER: 242 | in_cmd.init(c); // got command id 243 | state = STATE_GOT_ID; 244 | break; 245 | 246 | case STATE_GOT_ID: 247 | len = c; 248 | state = STATE_GOT_LEN; 249 | break; 250 | 251 | case STATE_GOT_LEN: 252 | if (c == (uint8_t)(in_cmd.id + len) && len <= sizeof(in_cmd.data)) { // checksum and size ok 253 | SerialCommand::init_checksum(checksum); 254 | state = len == 0 ? STATE_GOT_DATA : STATE_GOT_HEADER; 255 | } else { 256 | onParseError(); 257 | state = STATE_WAIT; 258 | } 259 | break; 260 | 261 | case STATE_GOT_HEADER: 262 | in_cmd.data[in_cmd.len++] = c; 263 | SerialCommand::update_checksum(checksum, c); 264 | if(in_cmd.len == len) state = STATE_GOT_DATA; 265 | break; 266 | 267 | case STATE_GOT_DATA: 268 | state = STATE_WAIT; 269 | 270 | if(c == checksum) { // checksum ok 271 | return 1; 272 | } else { 273 | onParseError(); 274 | } 275 | break; 276 | } 277 | 278 | return 0; 279 | } 280 | 281 | /* Parse and fill SerialCommand object SBGC_Parser.in_cmd; 282 | * Returns 1 if command is parsed, 0 otherwise. 283 | */ 284 | inline int8_t read_cmd() { 285 | while(com_obj->getBytesAvailable()) { 286 | if(process_char(com_obj->readByte())) { 287 | return 1; 288 | } 289 | } 290 | 291 | return 0; 292 | } 293 | 294 | 295 | /* 296 | * Formats and writes command to serial port. 297 | * If wait=1, waits even if output buffer is full. 298 | * If wait=0, writes only if output buffer has enough space to accept this command. 299 | * Returns 0 on success, PARSER_ERR_xx on fail. 300 | */ 301 | uint8_t send_command(uint8_t cmd_id, void *data, uint16_t size, uint8_t wait = 1) { 302 | if(com_obj != NULL && size <= (SBGC_CMD_MAX_BYTES - SBGC_CMD_NON_PAYLOAD_BYTES)) { 303 | if(wait || com_obj->getOutEmptySpace() >= size + SBGC_CMD_NON_PAYLOAD_BYTES) { 304 | com_obj->writeByte(SBGC_CMD_START_BYTE); // protocol-specific start marker 305 | com_obj->writeByte(cmd_id); // command id 306 | com_obj->writeByte(size); // data body length 307 | com_obj->writeByte(cmd_id + size); // header checksum 308 | 309 | // Write data 310 | uint8_t checksum; 311 | SerialCommand::init_checksum(checksum); 312 | for(uint8_t i = 0; i < size; i++) { 313 | com_obj->writeByte(((uint8_t*)data)[i]); 314 | SerialCommand::update_checksum(checksum, ((uint8_t*)data)[i]); 315 | } 316 | com_obj->writeByte(checksum); // data checksum 317 | 318 | return 0; 319 | } else { 320 | return PARSER_ERROR_BUFFER_IS_FULL; 321 | } 322 | } else { 323 | return PARSER_ERROR_WRONG_CMD_SIZE; 324 | } 325 | } 326 | 327 | /* 328 | * Send command from the SerialCommand object to serial port. See send_command() description. 329 | */ 330 | inline uint8_t send_cmd(SerialCommand &cmd, uint8_t wait = 1) { 331 | return send_command(cmd.id, cmd.data, cmd.len, wait); 332 | } 333 | 334 | 335 | /* 336 | * Get parsing errors counter 337 | */ 338 | inline uint16_t get_parse_error_count() { return parser_error_count; } 339 | 340 | 341 | /* 342 | * Resets the state of a parser 343 | */ 344 | inline void reset() { 345 | state = STATE_WAIT; 346 | } 347 | 348 | 349 | /* 350 | * Returns the space available in the output buffer 351 | */ 352 | inline uint16_t get_out_empty_space() { 353 | return com_obj != NULL ? com_obj->getOutEmptySpace() : 0; 354 | } 355 | 356 | }; 357 | 358 | 359 | 360 | #endif //__SBGC_parser__ -------------------------------------------------------------------------------- /libraries/SBGC_lib/include/SBGC_rc.h: -------------------------------------------------------------------------------- 1 | /* 2 | SimpleBGC Serial API library - RC (Remote Control) definitions 3 | More info: http://www.basecamelectronics.com/serialapi/ 4 | 5 | Copyright (c) 2014-2015 Aleksei Moskalenko 6 | All rights reserved. 7 | 8 | See license info in the SBGC.h 9 | */ 10 | #ifndef __SBGC_rc__ 11 | #define __SBGC_rc__ 12 | 13 | // RC channels used in the SBGC controller 14 | #define SBGC_RC_NUM_CHANNELS 6 // ROLL, PITCH, YAW, CMD, EXT_ROLL, EXT_PITCH 15 | 16 | // Hardware RC inputs, as labeled on the board 17 | #define SBGC_RC_INPUT_NO 0 18 | #define SBGC_RC_INPUT_ROLL 1 19 | #define SBGC_RC_INPUT_PITCH 2 20 | #define SBGC_RC_INPUT_EXT_ROLL 3 21 | #define SBGC_RC_INPUT_EXT_PITCH 4 22 | #define SBGC_RC_INPUT_YAW 5 // not connected in 1.0 board 23 | 24 | // Analog inputs (board v.3.0) 25 | #define SBGC_RC_INPUT_ADC1 33 26 | #define SBGC_RC_INPUT_ADC2 34 27 | #define SBGC_RC_INPUT_ADC3 35 28 | 29 | 30 | 31 | // Bit indicates input is in analog mode 32 | #define SBGC_RC_INPUT_ANALOG_BIT (1<<5) // 32 33 | // Bit indicates input is a virtual channel 34 | #define SBGC_RC_INPUT_VIRT_BIT (1<<6) // 64 35 | // Bit indicates input is a API virtual channel 36 | #define SBGC_RC_INPUT_API_VIRT_BIT (1<<7) // 128 37 | 38 | // Mask to separate input channel number from input mode 39 | #define SBGC_RC_INPUT_CH_MASK ((1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4)) 40 | #define SBGC_RC_INPUT_MODE_MASK ((1<<5) | (1<<6) | (1<<7)) 41 | 42 | 43 | // Number of virtual channels for RC serial input (s-bus, spektrum, Sum-PPM) 44 | #define SBGC_VIRT_NUM_CHANNELS 32 45 | // Number of virtual channels for API serial input 46 | #define SBGC_API_VIRT_NUM_CHANNELS 32 47 | 48 | 49 | // Normal range of RC signal. Real signal may go outside this range 50 | #define SBGC_RC_MIN_VAL -500 51 | #define SBGC_RC_MAX_VAL 500 52 | // Value to encode 'RC no signal' 53 | #define SBGC_RC_UNDEF -10000 54 | 55 | 56 | 57 | #endif // __SBGC_rc__ 58 | --------------------------------------------------------------------------------