ESP8266にドローンのふりをさせるスケッチ

dummy_drone.ino

#include <SoftwareSerial.h>
#include "path/to/mavlink.h"

// RX is digital pin 4 (connect to TX of other device)
// TX is digital pin 5 (connect to RX of other device)
SoftwareSerial mavSerial(4, 5);

const int system_id = 20;
const int component_id = MAV_COMP_ID_IMU;
const int type = MAV_TYPE_QUADROTOR;

void setup() {
  Serial.begin(9600);
  mavSerial.begin(9600);

  Serial.println("HELO!");
}

void loop() {
  uint8_t system_type = MAV_TYPE_FIXED_WING;
  uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC;

  uint8_t system_mode = MAV_MODE_PREFLIGHT;
  uint32_t custom_mode = 0;
  uint8_t system_state = MAV_STATE_STANDBY;

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot_type, system_mode, custom_mode, system_state);

  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  delay(1000);
  mavSerial.write(buf, len);
  delay(100);  // コンパニオンコンピュータ側からパケットが到着するのを少し待つ
  comm_receive();
}

void comm_receive() {  
  mavlink_message_t msg;
  mavlink_status_t status;

  while (mavSerial.available()) {
    uint8_t c = mavSerial.read();
    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
      // Handle message
      switch (msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:
          // do nothing
          break;
        case MAVLINK_MSG_ID_COMMAND_LONG:
          // do nothing
          break;
        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
          {
            Serial.println("MAVLINK_MSG_ID_PARAM_REQUEST_READ:");
            param_request_read_handler(&msg);
          }
          break;
        default:
          // do nothing
          break;
      }
    }
  }
}

void param_request_read_handler(mavlink_message_t *msg) {
  uint8_t target_system;
  uint8_t target_component;
  char _param_id[17];

  mavlink_msg_param_request_read_get_param_id(msg, _param_id);
  String param_id = String(_param_id);
  Serial.print("param_id: ");
  Serial.println(param_id);
  uint16_t param_index = mavlink_msg_param_request_read_get_param_index(msg);
  Serial.print("param_index: ");
  Serial.println(param_index);
  if (param_id == "VFR_HUD") {
    send_vfr_hud();
  }
  else {
    // do_nothing
  }
}

void send_vfr_hud() {
  Serial.println("send_vfr_hud()");

  float airspeed = 0.0;
  float groundspeed = 0.0;
  int16_t heading = 184;
  uint16_t throttle = 0;
  float alt = 21.58;
  float climb = -0.0;

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  uint16_t len;
  int send_bytes = 0;

  /*
   * static inline uint16_t mavlink_msg_vfr_hud_pack(
   *   uint8_t system_id,
   *   uint8_t component_id,
   *   mavlink_message_t* msg,
   *   float airspeed,
   *   float groundspeed,
   *   int16_t heading,
   *   uint16_t throttle,
   *   float alt,
   *   float climb
   * )
   */
  mavlink_msg_vfr_hud_pack(system_id, component_id, &msg, airspeed, groundspeed, heading, throttle, alt, climb);
  len = mavlink_msg_to_send_buffer(buf, &msg);
  send_bytes = mavSerial.write(buf, len);
  Serial.print("mavlink_msg_vfr_hud_pack send_bytes: ");
  Serial.println(send_bytes, DEC);
}