NanopixCore.cpp 26.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
/*
    Copyright (C) 2017 Nanopix
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in
      the documentation and/or other materials provided with the 
      distribution.
    * Neither the name of Nanopix nor the names
      of its contributors may be used to endorse or promote products
      derived from this software without specific prior written
      permission.
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
    OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 
    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
    OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
    SUCH DAMAGE.
*/

#include "NanopixCore.h"
31 32 33 34 35
#include "nanopix_config.h"
#include "utils.h"

#define MAV_RX_CHAN 0
#define MAV_TX_CHAN 1
36

37 38
#define BOOT_MODE_WAIT_UPGRADE_MS (5 * 60 * 1000)

Daniil Kukushkin's avatar
Daniil Kukushkin committed
39 40 41
#include <SoftwareSerial.h>
//SoftwareSerial readerSerial(0, SW_SERIAL_UNUSED_PIN); // RX, TX

42 43 44 45 46
void internal_initialization() // called from <arduino>\hardware\esp8266_2.3.0\2.3.0\cores\esp8266\core_esp8266_main.cpp
{
	NanopixCore::internal_init();
}

47
namespace NanopixCore {
48

49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97
void internal_init()
{
	SettingsManager & settingsManager = SettingsManager::getInstance();
	MainController & instance = MainController::getInstance();
	instance.initializeSystem();
	SettingsManager::BOOT_MODES bootMode = settingsManager.getNextBootMode();
	if(bootMode == SettingsManager::UPGRADE_BOOT_MODE)
	{
		float blueColorIntens = 80;
		int blueColor = (int)blueColorIntens;
		int blueColorIntensStep = 6;
		
		instance.setFrontLEDColor(0, 0, 80);
		instance.setBackLEDColor(0, 0, 80);
		settingsManager.setNextBootMode(SettingsManager::NORMAL_BOOT_MODE);
		long startMillis = millis();
		long nextReportMs = millis();
		long remainingTimeMs = 0;
		while(remainingTimeMs >= 0)
		{
			remainingTimeMs = startMillis + BOOT_MODE_WAIT_UPGRADE_MS - millis();
			ArduinoOTA.handle();

			if(blueColor <= 15 || blueColor >= 150)
				blueColorIntensStep = -blueColorIntensStep;
			blueColor += blueColorIntensStep;

			instance.setFrontLEDColor(0, 0, blueColor);
			instance.setBackLEDColor(0, 0, blueColor);
			
			settingsManager.parseMavPackets();
			
			if(millis() > nextReportMs)
			{
				char dropLog[64];
				sprintf(dropLog, "ESP in boot mode, will reboot in %d sec", (int)(remainingTimeMs / 1000));
				instance.sendLogMsg(dropLog);
				nextReportMs += 1000;
			}
			
			delay(30);
		}
		
		instance.setFrontLEDColor(255, 0, 0);
		instance.setBackLEDColor(0, 255, 0);
	}
}

MainController * mainControllerPtr = NULL;
98 99 100 101 102 103 104 105
volatile bool asyncUdpBlocked = false;
os_timer_t stmToAppRetransmitTimer;
uint8_t prevPacketSeq = 0;
int dropCount = 0;
int mavPacketsReceived = 0;
volatile bool isControlStationConnected = false;
uint8_t txPacketsSeqCounter = 0;

106
void udpOnPacketHandler(AsyncUDPPacket packet);
107

108 109
void WiFiEvent(WiFiEvent_t event);

110 111 112 113 114 115 116 117 118 119 120
MainController * MainController::instance = NULL;
MainController & MainController::getInstance()
{
	if(instance == NULL)
	{
		instance = new MainController();
	}
	return *instance;
}

MainController::MainController() : settingsManager(SettingsManager::getInstance())
121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
{    
  asyncUdpBlocked = false;
  mainControllerPtr = NULL;
  prevPacketSeq = 0;
  dropCount = 0;
  mavPacketsReceived = 0;
  isControlStationConnected = 0;
  txPacketsSeqCounter = 0;
  
  attitudeControlMode = FusionMode;
  flightScriptIsBlocked = false;
  
  memset(mavSendBuffer, 0, sizeof(mavSendBuffer));

  isSomeConnectedBefore = false;
  noConnectSeconds = 0;

  currentMillis = millis();
  prevMillis = 0;
  measureIntervalMillis = 1000;
  
  memset(pixReceiverBuffer, 0, sizeof(pixReceiverBuffer));

  isManualControlOverrided = false;
145 146 147 148
  pitchControl = 0;
  rollControl = 0;
  throttleControl = 0;
  yawControl = 0;
149 150
  flightScriptIsBlocked = false;
  motorsArmCommandReceived = false;
151

152 153 154 155
  currentPosZ = 0;
  currentRoll = 0;
  currentPitch = 0; 
  currentYaw = 0;
156 157 158 159
  rcRoll = 1500;
  rcPitch = 1500;
  rcThrottle = 1500;
  rcYaw = 1500;
160

161
  wifiChannel = 0;
162 163
}

164 165 166 167 168 169 170 171
MainController::~MainController()
{
	if(instance != NULL)
	{
		delete instance;
	}
}

172 173
void MainController::openAsyncUDP()
{
174 175
  if(Udp.listen(localUdpPort))
  {
176
#if DEBUG  
177
      Serial.println(WiFi.localIP());
178
#endif
179 180 181 182 183 184 185 186 187 188 189 190
      Udp.onPacket(udpOnPacketHandler);  
  }
}

void MainController::closeAsyncUDP()
{
  Udp.close();
}

void retransmitTimerCallback(void *pArg) {
   if(asyncUdpBlocked)
      return;
191 192
   if(mainControllerPtr != NULL)
      mainControllerPtr->grabSerialAndRetransmit();
193 194
}

195 196 197 198 199 200 201 202 203 204
void startRetransmitTimer()
{
	os_timer_arm(&stmToAppRetransmitTimer, 1000 / RETRANSMIT_TIMER_FREQ, true);
}

void stopRetransmitTimer()
{
	os_timer_disarm(&stmToAppRetransmitTimer);
}

205 206
void MainController::initializeSystem()
{
207 208
  mainControllerPtr = this;
  
209 210
  uint8_t offer_router_mode = 0;

211 212
  WiFi.setOutputPower(20.5);
  
213
  bool res;
214 215 216
  int ret = NANOPIX_STATUS_OK;
  IPAddress localIP;

217 218 219
  isSomeConnectedBefore = false;
  noConnectSeconds = 0;
 
Daniil Kukushkin's avatar
Daniil Kukushkin committed
220 221 222
  //Serial1.begin(38400);
  //readerSerial.begin(38400);
 
223
  Serial.begin(DEBUG_BAUDRATE);
224
  delay(500);
225 226 227

  srand(ESP.getCycleCount());
  
228 229
  NP_CHECK(udpSender.initialize(&Udp));

230 231
  delay(200);
  WiFi.mode(WIFI_AP);
232
  WiFi.softAPConfig(udpSender.ip, udpSender.gateway, udpSender.subnet);
233

234
  settingsManager = SettingsManager::getInstance();
235
  settingsManager.initAll(&udpSender);
236 237 238
  wifiChannel = settingsManager.getWifiChannel();
  if(wifiChannel == 0)
    wifiChannel = selectOptimalWifiChannel();
239

240 241 242
  res = WiFi.softAP(settingsManager.SSID.c_str(), settingsManager.PASS.c_str(), wifiChannel);
  Serial.printf("SoftAP %s:%s on channel %d", settingsManager.SSID.c_str(), settingsManager.PASS.c_str(), wifiChannel);
  Serial.println(res ? " READY" : " START FAILURE");
243

244 245
  wifi_softap_set_dhcps_offer_option(OFFER_ROUTER, &offer_router_mode);

246
  //WiFi.softAP("quadcopter", "testpass");
247 248
  //WiFi.setOutputPower(10);
  
249 250
  openAsyncUDP();
  
251
  STM_OTA_UDP_Receiver::start(STM32_OTA_UDP_PORT, STM32_OTA_SERVER_UDP_PORT);
252 253 254

  WiFi.onEvent(WiFiEvent);
  localIP = WiFi.softAPIP();
255
#if DEBUG  
256 257 258 259
  Serial.print("My IP: ");
  Serial.println(localIP.toString().c_str());
  Serial.print("Local IP: ");
  Serial.println(WiFi.localIP());
260
#endif
261 262 263

  ArduinoOTA.onStart([]() {
    //Serial.println("Start");
264 265
    STM_BootManager::toResetState();
    
266 267 268
  });
  ArduinoOTA.onEnd([]() {
    //Serial.println("\nEnd");
269
    STM_BootManager::reset();
270 271 272 273 274 275 276 277 278 279 280 281 282 283
  });
  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    //Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
  });
  ArduinoOTA.onError([](ota_error_t error) {
    /*Serial.printf("Error[%u]: ", error);
    if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
    else if (error == OTA_END_ERROR) Serial.println("End Failed");*/
  });
  ArduinoOTA.begin();

284
  os_timer_setfn(&stmToAppRetransmitTimer, retransmitTimerCallback, NULL);
285 286 287 288
  startRetransmitTimer();
out:
    if(ret != NANOPIX_STATUS_OK)
        Serial.printf("Initialization failure, code: %d", ret);
289 290 291
  delay(50);
  Serial.end();
  Serial.begin(UART_BAUDRATE);
292 293
}

294

295
void udpOnPacketHandler(AsyncUDPPacket packet) {
296 297 298
  if(asyncUdpBlocked)
    return;
    
299
  uint8_t* packetData = packet.data();
300 301 302 303
  size_t dataLength = packet.length();

  memcpy(mainControllerPtr->udpRxBuffer, packetData, dataLength);

304
  IPAddress addr = packet.remoteIP();
305

306 307 308
  if(mainControllerPtr != NULL)
  {
    for(int i = 0; i < dataLength; i++) {
309
      uint8_t res = mavlink_parse_char(MAV_RX_CHAN, mainControllerPtr->udpRxBuffer[i], &(mainControllerPtr->rxMessage), &(mainControllerPtr->mavStatus) );
310 311 312 313 314 315
      if(res) {
         mainControllerPtr->isSomeConnectedBefore = true; 
        
         uint8_t packID = mainControllerPtr->rxMessage.msgid;
         uint8_t packetSeq = mainControllerPtr->rxMessage.seq;
  
316
         isControlStationConnected = true;
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358
  
         mavPacketsReceived++;
         uint8_t dif = packetSeq - prevPacketSeq;
         dropCount += dif - 1;
   #if DEBUG      
         /*Serial.print(packID);
         Serial.print(" | ");
         Serial.print(mainControllerPtr->rxMessage.seq);
         Serial.print(" | ");
         Serial.write(packetData, dataLength);
         Serial.println();*/
  #endif
         
         prevPacketSeq = packetSeq;
         
         switch(packID) {
          case MAVLINK_MSG_ID_MANUAL_CONTROL:
            mavlink_msg_manual_control_decode(&(mainControllerPtr->rxMessage), &(mainControllerPtr->manualControlMsg) );
  
            mainControllerPtr->rcRoll = mainControllerPtr->manualControlMsg.x;
            mainControllerPtr->rcPitch = mainControllerPtr->manualControlMsg.y; 
            mainControllerPtr->rcThrottle = mainControllerPtr->manualControlMsg.z;
            mainControllerPtr->rcYaw = mainControllerPtr->manualControlMsg.r;
  
            // override data in manual control packet:
            //if(mainControllerPtr->isManualControlOverrided) {
            mavlink_message_t msg;
  
            int rollRC, pitchRC, throttleRC, yawRC;
            switch(mainControllerPtr->attitudeControlMode) {
              case MainController::OverrideMode:
                rollRC = mainControllerPtr->rollControl * 1000 + 1500;
                pitchRC = mainControllerPtr->pitchControl * 1000 + 1500;
                throttleRC = mainControllerPtr->throttleControl * 1000 + 1000;
                yawRC = mainControllerPtr->yawControl * 1000 + 1500;
              break;
              case MainController::FusionMode:
                rollRC = mainControllerPtr->rcRoll + mainControllerPtr->rollControl * 1000;
                pitchRC = mainControllerPtr->rcPitch + mainControllerPtr->pitchControl * 1000;
                throttleRC = mainControllerPtr->rcThrottle + mainControllerPtr->throttleControl * 1000;
                yawRC = mainControllerPtr->rcYaw + mainControllerPtr->yawControl * 1000;
              break;
359
            }
360 361
            
            mavlink_msg_manual_control_pack(0, 0, &msg, 0, rollRC, pitchRC, throttleRC, yawRC, 0);
362
            mavlink_msg_to_send_buffer(mainControllerPtr->udpRxBuffer, &msg);
363 364 365 366
            //}
  
            /*if( abs(mainControllerPtr->rcRoll - 1500) > 30 || abs(mainControllerPtr->rcPitch - 1500) > 30
            || abs(mainControllerPtr->rcThrottle - 1500) > 30 || abs(mainControllerPtr->rcYaw - 1500) > 30 )
367
            {
368 369 370 371 372 373 374 375 376 377
              if(mainControllerPtr->isManualControlOverrided) {
                mainControllerPtr->flightScriptIsBlocked = true;
                mainControllerPtr->isManualControlOverrided = false;
              }
            }*/
          break;
          case MAVLINK_MSG_ID_COMMAND_LONG:
            mavlink_command_long_t commandLongMsg;
            mavlink_msg_command_long_decode(&(mainControllerPtr->rxMessage), &(commandLongMsg) );
  
378
            if(commandLongMsg.command == MAV_CMD_COMPONENT_ARM_DISARM) 
379 380 381 382 383 384 385 386
            {
              if(commandLongMsg.param1 > 0.5f) // then arm command 
              {
                mainControllerPtr->motorsArmCommandReceived = true;
                mainControllerPtr->flightScriptIsBlocked = false;
              } else {
                //mainControllerPtr->flightScriptIsBlocked = true;
              }
387
            }
388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405
            
            if(commandLongMsg.command == MAV_CMD_MISSION_START) 
            {
              mainControllerPtr->missionStartCmdReceived = true;
            }
            
            if(commandLongMsg.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) 
            {
              if(commandLongMsg.param2 == 1)
              {
                ESP.restart();
              }
              if(commandLongMsg.param2 == 3)
              {
                mainControllerPtr->settingsManager.setNextBootMode(SettingsManager::UPGRADE_BOOT_MODE);
                ESP.restart();
              }
            }
406 407 408 409
            
          break;
  
          default:
410
          
411 412 413 414
          break;
         }
  
  #if not DEBUG
415
         //if(!(packID == MAVLINK_MSG_ID_MANUAL_CONTROL/* && !mainControllerPtr->udpSender.isMainController(addr)*/))
416
         {
417
            Serial.write(mainControllerPtr->udpRxBuffer, dataLength);
418 419 420 421
         }
  #endif
      }
    } 
422 423
  }

424
  if((mainControllerPtr != NULL) && dataLength)
425 426 427 428
  {
    if(!mainControllerPtr->udpSender.hasController(addr))
        mainControllerPtr->udpSender.registerController(addr);
  }
429 430
}

431 432 433 434 435 436 437
bool MainController::isMissionTriggered()
{
	bool val = missionStartCmdReceived;
	missionStartCmdReceived = false;
	return val;
}

438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489
void MainController::setAttitudeControlMode(AttitudeControlMode attitudeCtrlMode)
{
  attitudeControlMode = attitudeCtrlMode;
}

int MainController::getAltitude()
{
  return currentPosZ / 10;
}

bool MainController::isArmCommandReceived()
{
  bool res = motorsArmCommandReceived;
  motorsArmCommandReceived = false;
  return res;
}

void MainController::setPitch(float radAngle)
{
  if(flightScriptIsBlocked)
    return;
  isManualControlOverrided = true;
  pitchControl = radAngle;
}

void MainController::setRoll(float radAngle)
{
  if(flightScriptIsBlocked)
    return;
  isManualControlOverrided = true;
  rollControl = radAngle;
}
void MainController::setYawSpeed(float radSecSpeed)
{
  if(flightScriptIsBlocked)
    return;
  isManualControlOverrided = true;
  yawControl = radSecSpeed;
}
void MainController::setThrottle(float normalizedThrottle)
{
  if(flightScriptIsBlocked)
    return;
  isManualControlOverrided = true;
  throttleControl = normalizedThrottle;
}

void MainController::setParam(const char* name, float value)
{
  mavlink_message_t msg;
  mavlink_msg_param_set_pack(0, 0, &msg, 0, 0, name, value, MAV_PARAM_TYPE_REAL32);
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
490 491 492 493 494
  for(int i = 0; i < 5; i++)
  {
	Serial.write(mavSendBuffer, txSize);
	delayMicroseconds(1);
  }
495 496
}

497
int MainController::sendMavlinkPacket(mavlink_message_t* msg, bool multicast)
498
{
499 500 501 502 503 504 505 506 507 508 509 510
    int ret = NANOPIX_STATUS_OK;
    int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, msg);
    if(multicast || (udpSender.connectedCount() == 0))
    {
        NP_CHECK(udpSender.sendMulticast(mavSendBuffer, txSize));
    }
    else
    {
        NP_CHECK(udpSender.sendToConnected(mavSendBuffer, txSize));
    }
out:
    return ret;
511 512
}

513
int MainController::sendLogMsg(const char* str, bool multicast)
514
{
515
  int ret = NANOPIX_STATUS_OK;
516 517 518
  uint8_t chan = 1;
  static mavlink_message_t msg;
  mavlink_msg_statustext_pack_chan(0,0,chan, &msg, 0, str);
519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541
  NP_CHECK(sendMavlinkPacket(&msg, multicast));
out:
  return ret;
}

int MainController::sendLogMsg(int value, bool multicast)
{
  int ret = NANOPIX_STATUS_OK;
  char dropLog[32];
  sprintf(dropLog, "%d", value);
  NP_CHECK(sendLogMsg(dropLog, multicast));
out:
  return ret;
}

int MainController::sendLogMsg(float value, bool multicast)
{
  int ret = NANOPIX_STATUS_OK;
  char dropLog[32];
  sprintf(dropLog, "%f", value);
  NP_CHECK(sendLogMsg(dropLog, multicast));
out:
  return ret;
542 543
}

544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561
void MainController::sendToUART(const uint8_t* data, uint16_t count)
{
  static mavlink_message_t msg;
  static uint8_t packetData[128];
  int index = 0;
  while(index < count)
  {
    int datasize = MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN;
    if(count - index < MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN)
      datasize = count - index;
    mavlink_msg_log_data_pack_chan(0, 0, 0, &msg, 0, 0, datasize, &(data[index]));
    int16_t txSize = mavlink_msg_to_send_buffer(packetData, &msg);
    Serial.write(packetData, txSize);
    index += datasize;
  }

}

562
void MainController::setLEDsColors(uint8_t forwardR, uint8_t forwardG, uint8_t forwardB, uint8_t backwardR, uint8_t backwardG, uint8_t backwardB)
Daniil Kukushkin's avatar
Daniil Kukushkin committed
563 564 565 566 567
{
  setFrontLEDColor(forwardR, forwardG, forwardB);
  setBackLEDColor(backwardR, backwardG, backwardB);
}

568
void MainController::setLEDsColors(Color & forwardColor, Color & backwardColor)
Daniil Kukushkin's avatar
Daniil Kukushkin committed
569 570 571 572 573
{
  setLEDsColors(forwardColor.R, forwardColor.G, forwardColor.B, backwardColor.R, backwardColor.G, backwardColor.B);
}

void MainController::setFrontLEDColor(uint8_t forwardR, uint8_t forwardG, uint8_t forwardB)
574 575 576 577 578 579
{
  LedColor fc;
  fc.R = forwardR;
  fc.G = forwardG;
  fc.B = forwardB;
  fc.mode = COLOR_NOT_SAVE_MODE;
Daniil Kukushkin's avatar
Daniil Kukushkin committed
580 581 582 583 584
  setParam("FrontLed", fc.floatParamValue);
}

void MainController::setBackLEDColor(uint8_t backwardR, uint8_t backwardG, uint8_t backwardB)
{
585 586 587 588 589
  LedColor bc;
  bc.R = backwardR;
  bc.G = backwardG;
  bc.B = backwardB;
  bc.mode = COLOR_NOT_SAVE_MODE;
590
  setParam("BackLed", bc.floatParamValue);
591 592
}

593 594 595 596 597 598 599 600 601 602
void MainController::setFrontLEDColor(Color & forwardColor)
{
	setFrontLEDColor(forwardColor.R, forwardColor.G, forwardColor.B);
}

void MainController::setBackLEDColor(Color & backwardColor)
{
    setBackLEDColor(backwardColor.R, backwardColor.G, backwardColor.B);
}

603 604 605 606 607 608 609 610 611 612 613
void MainController::switchArmDisarm(bool armState)
{
  //if(flightScriptIsBlocked)
  //  return;
  mavlink_message_t msg;
  mavlink_msg_command_long_pack(0, 0, &msg, 0, 0, MAV_CMD_COMPONENT_ARM_DISARM, 0, (armState ? 1.0f : 0.0f), 0, 0, 0, 0, 0, 0);
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
  for(int i = 0; i < 10; i++)
    Serial.write(mavSendBuffer, txSize);
}

614 615 616 617 618 619 620 621
void MainController::setMotorSpeed(uint8_t motorNumber, float speedPercent)
{
  mavlink_message_t msg;
  mavlink_msg_command_long_pack(0, 0, &msg, 0, 0, MAV_CMD_DO_MOTOR_TEST, 0, motorNumber, MOTOR_TEST_THROTTLE_PERCENT, speedPercent, 0, 0, 0, 0);
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
  Serial.write(mavSendBuffer, txSize);
}

622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671
void MainController::armMotors()
{
  switchArmDisarm(true);
}

void MainController::disarmMotors()
{
  switchArmDisarm(false);
}

void MainController::sendCommandIntMavMsg(uint8_t msgType, float param1=0, float param2=0, float param3=0, float param4=0, float param5=0, float param6=0, float param7=0)
{
  mavlink_message_t msg;
  mavlink_msg_command_int_pack(0, 0, &msg, 0, 0, 0, msgType, 0, 0, param1, param2, param3, param4, param5, param6, param7);
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
  for(int i = 0; i < 5; i++)
    Serial.write(mavSendBuffer, txSize);
}

void MainController::rebootFlightController()
{
  if(flightScriptIsBlocked)
    return;
  sendCommandIntMavMsg(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1);
}

void MainController::setTargetAltitude(int centimeters)
{
  //if(flightScriptIsBlocked)
  //  return;
  sendCommandIntMavMsg(MAV_CMD_DO_CHANGE_ALTITUDE, centimeters/100.f);
}


void MainController::switchTakeoffLand(bool takeoff)
{
  if(flightScriptIsBlocked)
    return;
  mavlink_message_t msg;
  uint8_t commandByte;
  if(takeoff)
      commandByte = MAV_CMD_NAV_TAKEOFF;
  else
      commandByte = MAV_CMD_NAV_LAND;
  mavlink_msg_command_long_pack(0, 0, &msg, 0, 0, commandByte, 0, 0, 0, 0, 0, 0, 0, 0);
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
  for(int i = 0; i < 5; i++)
    Serial.write(mavSendBuffer, txSize);
}

672
void MainController::requestTelemetryDataStream(TelemetryPacketType streamType, int streamRateHz, bool startStopFlag)
673 674
{
  mavlink_message_t msg;
675
  mavlink_msg_request_data_stream_pack(0, 0, &msg, 0, 0, streamType, streamRateHz, startStopFlag);
676 677 678 679 680 681 682 683 684 685 686 687 688 689 690
  int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &msg);
  for(int i = 0; i < 5; i++)
    Serial.write(mavSendBuffer, txSize);
}

void MainController::takeoff()
{
  switchTakeoffLand(true);
}

void MainController::land()
{
  switchTakeoffLand(false);
}

691
void MainController::grabSerialAndRetransmit()
692
{
693 694 695 696
    int availableBytes = Serial.available();
    int pixReceiverCount = availableBytes > PIX_RECEIVER_BUFFER_SIZE ? PIX_RECEIVER_BUFFER_SIZE : availableBytes;
    Serial.readBytes(pixReceiverBuffer, pixReceiverCount);

697
    if(mainControllerPtr != NULL)
698
    {
699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726
      for(int i = 0; i < pixReceiverCount; i++) 
      {
          uint8_t packetCompleted = mavlink_parse_char(MAV_TX_CHAN, pixReceiverBuffer[i], &(mainControllerPtr->rxMsgFromFC), &(mainControllerPtr->mavStatusFC) );
  
          if(packetCompleted) 
          {
              int16_t txSize = mavlink_msg_to_send_buffer(mavSendBuffer, &(mainControllerPtr->rxMsgFromFC) );
              if (udpSender.connectedCount() == 0)
                  udpSender.sendMulticast(mavSendBuffer, txSize);
              else
                  udpSender.sendToConnected(mavSendBuffer, txSize);
        
  
              uint8_t packID = mainControllerPtr->rxMsgFromFC.msgid;
              switch(packID) {
              case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
                  mavlink_local_position_ned_t localPosMsg;
                  mavlink_msg_local_position_ned_decode(&(mainControllerPtr->rxMsgFromFC), &localPosMsg );
                  mainControllerPtr->currentPosZ = localPosMsg.z;
              break;
              case MAVLINK_MSG_ID_ATTITUDE:
                  mavlink_attitude_t attitudeMsg;
                  mavlink_msg_attitude_decode(&(mainControllerPtr->rxMsgFromFC), &attitudeMsg );
            
                  mainControllerPtr->currentRoll = attitudeMsg.roll;
                  mainControllerPtr->currentPitch = attitudeMsg.pitch;
                  mainControllerPtr->currentYaw = attitudeMsg.yaw;
              break;
727 728 729 730 731 732 733 734 735
              case MAVLINK_MSG_ID_LOG_DATA:
                  mavlink_log_data_t logDataMsg;
                  mavlink_msg_log_data_decode(&(mainControllerPtr->rxMsgFromFC), &logDataMsg);
                  for(int i = 0; i < logDataMsg.count; i++)
                  {
                      if(!uartBridgeBuffer.add(logDataMsg.data[i]))
                        sendLogMsg("UART bridge RX buffer overflowed");
                  }
              break;
736 737 738 739 740 741
              default:
              break;
              }
         
          }
      }
742
    }
743 744
}

745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760
size_t MainController::availableBytesOnUART()
{
    return uartBridgeBuffer.numElements();
}

uint16_t MainController::readFromUART(uint8_t * readBuffer, uint16_t readCount)
{
    uint16_t count = 0;
    for(int i = 0; i < readCount && !uartBridgeBuffer.isEmpty(); i++)
    {
         uartBridgeBuffer.pull(&readBuffer[i]);
         count = i + 1;
    }
    return count;
}

761
void MainController::mainServiceRoutine()
762
{
763
  if(STM_OTA_UDP_Receiver::isOTA_Request())
764
  {
765
    stopRetransmitTimer();
766
    asyncUdpBlocked = true;
767 768
    STM_OTA_UDP_Receiver::handle();
    
769 770
    Serial.end();
    Serial.begin(UART_BAUDRATE);
771

772
    asyncUdpBlocked = false;
773
    startRetransmitTimer();
774
    STM_OTA_UDP_Receiver::restart();
775
  }
776
  
777 778
  ArduinoOTA.handle();

779
  settingsManager.parseMavPackets();
780 781

  currentMillis = millis();
782 783 784 785
  if (currentMillis - prevMillis >= measureIntervalMillis) 
  {
    if(isSomeConnectedBefore == false) 
    {
786 787 788 789 790 791
      noConnectSeconds++;
      //settingsManager.sendString( "No connect seconds: " + String(noConnectSeconds) );
      //Serial.println(noConnectSeconds);
      if( (noConnectSeconds > RESET_SSID_PASS_SECONDS) && (!settingsManager.isDefaultPasswordNow()) )
      {
         settingsManager.resetSSID_PassToDefault();
792
         STM_BootManager::toResetState();
793
         delay(2000);
794
         STM_BootManager::reset();
795 796 797 798 799
         ESP.restart();
      }
    } else
      noConnectSeconds = 0;

800 801 802 803 804
    int dropRate = 0;
    if(mavPacketsReceived > 0)
      dropRate = 100 * (dropCount / (float)mavPacketsReceived);
          
    char dropLog[128];
805 806
    sprintf(dropLog, "ESP Rx: %d, drop rate: %d% : ch: %d", mavPacketsReceived, dropRate, wifiChannel);
    sendLogMsg(dropLog, true);
807
      
Daniil Kukushkin's avatar
Daniil Kukushkin committed
808 809
	//Serial1.println("serial1");
	  
810
#if DEBUG
811
    /*Serial.print("----------------- UDP PACKETS RECEIVED: ");
812 813 814 815 816
    Serial.print(mavPacketsReceived);
    Serial.print(" | DROPED: ");
    Serial.print(dropCount);
    Serial.print(" | DROP RATE: ");
    Serial.print(dropRate);
817
    Serial.println("%");*/
818 819 820 821
#endif
    dropCount = 0;
    mavPacketsReceived = 0;

822 823 824
    prevMillis = millis();
  }

Daniil Kukushkin's avatar
Daniil Kukushkin committed
825 826 827 828 829
  /*while(readerSerial.available())
  {
	Serial1.write(readerSerial.read());
  }*/

830 831 832 833 834 835 836 837 838
  /*int bytesToRead = availableBytesOnUART();
  if(bytesToRead > 0)
  {
      uint8_t readBuf[64];
      int readCount = readFromUART(readBuf, 64);
      //sendLogMsg((char*)readBuf);
      sendToUART(readBuf, readCount);
  }*/

839 840 841
  yield();
}

842 843
void MainController::mainRoutine()
{
844 845
  do 
  {
846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881
    mainServiceRoutine();
  } while (isControlStationConnected == false);
}

int MainController::getRC_Roll()
{
  return rcRoll;
}
int MainController::getRC_Pitch()
{
  return rcPitch;
}
int MainController::getRC_Throttle()
{
  return rcThrottle;
}
int MainController::getRC_Yaw()
{
  return rcYaw;
}

float MainController::getRoll()
{
  return currentRoll;
}

float MainController::getPitch()
{
  return currentPitch;
}

float MainController::getYaw()
{
  return currentYaw;
}

882
void WiFiEvent(WiFiEvent_t event) {
883
#if DEBUG
884 885 886 887 888
  Serial.printf("[WiFi-event] event: %d\n", event);
#endif
  switch (event)
  {
    case WIFI_EVENT_STAMODE_CONNECTED:
889
#if DEBUG
890 891 892 893
      Serial.println("[ST]WiFi just connected");
#endif
      break;
    case WIFI_EVENT_STAMODE_DISCONNECTED:
894
#if DEBUG
895 896 897 898
      Serial.println("[ST]WiFi lost connection");
#endif
      break;
    case WIFI_EVENT_STAMODE_AUTHMODE_CHANGE:
899
#if DEBUG
900 901 902 903
      Serial.println("[ST]WiFi mode changed");
#endif
      break;
    case WIFI_EVENT_STAMODE_GOT_IP:
904
#if DEBUG
905 906 907 908 909
      Serial.print("[ST]WiFi connected, IP=");
      Serial.println(WiFi.localIP());
#endif
      break;
    case WIFI_EVENT_STAMODE_DHCP_TIMEOUT:
910
#if DEBUG
911 912 913 914
      Serial.println("[ST]WiFi DHCP TIMEOUT");
#endif
      break;
    case WIFI_EVENT_SOFTAPMODE_STACONNECTED:
915
#if DEBUG
916
      Serial.println("[AP] new client");
917
      printClientsStatus();
918 919 920
#endif
      break;
    case WIFI_EVENT_SOFTAPMODE_STADISCONNECTED:
921
#if DEBUG
922 923 924 925
      Serial.println("[AP]client disconnected");
#endif
      break;
    case WIFI_EVENT_SOFTAPMODE_PROBEREQRECVED:
926 927 928
#if DEBUG
      Serial.println("[AP]err on recieved request");
#endif
929 930 931 932 933
      break;
  }

}

934 935
}