ชมรมโดรน

2. แพ็กเก็ต I2C ทีละไบต์

อ่านแพ็กเก็ต 6 ไบต์ m_i2cData ตัวนับ และตาราง F_* จาก protocol.cpp และ protocol.hpp

ทุกครั้งที่คุณเรียก drone.takeOff() หรือ drone.fw(250) โค้ดของชมรมไม่ได้ "ส่ง" อะไรออกไปทันที มันเขียนค่าลงตัวแปรในหน่วยความจำของ Arduino แล้วรอให้ Flight Controller มา อ่าน

คิดเหมือนกระดานสั่งอาหารที่ห้องครัว พนักงานเขียนรายการลงกระดานแล้วเปลี่ยนเลขใบสั่งที่มุมกระดาน ห้องครัวเดินมาดูเมื่อสะดวก ถ้าเลขใบสั่งต่างจากที่จำไว้ แปลว่ามีรายการใหม่ ถ้าเลขเท่าเดิม ก็ไม่ต้องทำซ้ำ

การคุยกันของ Arduino กับ Flight Controller ผ่าน I2CArduino เก็บคำสั่ง 6 ไบต์ไว้ให้ถูกอ่าน รับปุ่มจากรีโมท และอ่าน 8 ไบต์จาก Flight Controller ที่ 0x34ArduinoWire.begin(0x33)m_i2cData[6]อุปกรณ์ที่อ่านคำสั่งปุ่มจากรีโมทFC · 0x34onRequest: ส่ง 6 ไบต์onReceive: เก็บไบต์สุดท้ายเป็น m_remoteKeyrequestFrom(0x34, 8)ไบต์ 0–1 = ความสูง (ซม.)ที่มา: Wire และ iicRecv() ใน protocol.cpp
Arduino เป็น slave ที่อยู่ 0x33 ถือกระดาน 6 ไบต์ไว้ให้ถูกอ่าน รับปุ่มจากรีโมท และไปอ่านความสูง 8 ไบต์จาก Flight Controller ที่ 0x34

หน้าตาของแพ็กเก็ต

protocol.cpp ประกาศแพ็กเก็ตไว้ 6 ไบต์ ส่วน protocol.hpp ประกาศรหัสคำสั่ง

char Arduino::m_i2cData[6];

void Arduino::requestEvent(void) {
  Wire.write(m_i2cData);        // ใครมาอ่าน ก็ส่ง 6 ไบต์นี้ให้
}
ไบต์หน้าที่
[0]ตัวนับ บวกหนึ่งทุกครั้งที่มีคำสั่งใหม่ (m_i2cData[0]++)
[1]รหัสคำสั่ง F_*
[2] ถึง [5]พารามิเตอร์ ใช้ตามคำสั่ง

ตัวนับเป็น char จึงวนกลับเมื่อเต็ม ไม่เป็นปัญหา เพราะสิ่งที่ฝั่งอ่านสนใจคือ "ต่างจากครั้งก่อนไหม" โค้ดฝั่ง Flight Controller ไม่มีให้ดู ข้อนี้เป็นการอ่านเจตนาจากรูปแบบโค้ด ไม่ใช่การยืนยันจากฝั่ง FC

ตัวอย่างทีละคำสั่ง

take_off() — ขึ้นบิน

  1. n+1ตัวนับ
  2. 01F_ARM
  3. –[2]
  4. –[3]
  5. –[4]
  6. –[5]

set_direction(1, 2) — เดินหน้า ความเร็วปกติ

  1. n+1ตัวนับ
  2. 0FF_DIRECTION
  3. 01ทิศ
  4. 02ความเร็ว
  5. –[4]
  6. –[5]

set_height(120) — สูง 120 ซม. (120 = 0x0078)

  1. n+1ตัวนับ
  2. 0EF_POS
  3. 78z ต่ำ
  4. 00z สูง
  5. –[4]
  6. –[5]

set_yaw(1, 90) — หมุน 90° (90 = 0x005A)

  1. n+1ตัวนับ
  2. 10F_YAW_ROTATE
  3. 01ทิศหมุน
  4. 5Aมุม ต่ำ
  5. 00มุม สูง
  6. –[5]

motor(1, 50) — มอเตอร์ 1 ที่ 50

  1. n+1ตัวนับ
  2. 0CF_MOTOR
  3. 01หมายเลข
  4. 32ความเร็ว
  5. –[4]
  6. –[5]

ช่อง "–" คือไบต์ที่คำสั่งนั้นไม่ได้เขียน มันคงค่าเดิมจากคำสั่งก่อนหน้า (ไม่ใช่ศูนย์) ซึ่งเป็นพฤติกรรมของโค้ด เพราะฟังก์ชันเขียนทับเฉพาะไบต์ที่ใช้

ค่า 16 บิต เช่นความสูงและมุม แบ่งเป็นไบต์ต่ำก่อนไบต์สูง (little-endian)

void Arduino::set_height(uint16_t z) {
  m_i2cData[0]++;
  m_i2cData[1] = Arduino::F_POS;
  m_i2cData[2] = (uint16_t)(z & 0xffff);        // ไบต์ต่ำ
  m_i2cData[3] = (uint16_t)(z & 0xffff) >> 8;   // ไบต์สูง
}

ตาราง F_*

จาก enum _FUNCTION_SEND ใน protocol.hpp ค่าที่ไม่ได้ระบุเลขตรงๆ นับต่อจากตัวก่อนหน้า

ชื่อเลขฟังก์ชันที่ใช้เขียนไบต์
F_NONE0ไม่ใช้
F_ARM1take_off()[1]
F_DISARM2stop_motor()[1]
F_AUTO_LAND3auto_land()[1]
F_CALIBRATION4calibration()[1]
F_ROTATE5rotate()[1]
F_FLIP6flip() ว่างเปล่า ไม่ส่งแพ็กเก็ต
F_SPEED7ประกาศไว้ ไม่มีฟังก์ชันเรียก
F_CIRCLE8circle_360()[1]
F_JUMP9jump()[1]
F_MOTOR12motor(num, speed)[2], [3]
F_POS14set_pos(x, y) และ set_height(z)[2]–[5]
F_DIRECTION15set_direction(dir, speed)[2], [3]
F_YAW_ROTATE16set_yaw(rotate, angle)[2]–[4]
F_PICTURE31picture()[1]
F_VIDEO_BEGIN32video_begin()[1]
F_VIDEO_END33video_end()[1]
F_CLOSE_JUMP37jump_end()[1]

สองข้อน่าสังเกต

  1. flip() ใน protocol.cpp มีตัวฟังก์ชันแต่ ไม่มีโค้ดข้างใน เรียก drone.flip() แล้วไม่มีอะไรเกิดขึ้น
  2. set_pos(x, y) กับ set_height(z) ใช้รหัส F_POS เดียวกัน และเขียนไบต์ [2],[3] ที่เดียวกัน (x กับ z ทับกัน) Flight Controller แยกออกอย่างไรไม่มีข้อมูล จึงไม่ควรเรียกสองอย่างนี้สลับกันโดยไม่ทดสอบ

รหัสทิศและความเร็ว

ค่า dir และ speed ที่ set_direction(dir, speed) ใช้ ตามคอมเมนต์ใน protocol.cpp

dirความหมายที่มา
1เดินหน้าmove_forward_cm
2ถอยmove_backward_cm
3ซ้ายmove_left_cm
4ขวาmove_right_cm
5ขึ้นup()
6ลงdown()
7หยุดนิ่ง ล็อกตำแหน่งposhold()

ความเร็ว 1, 2, 3 คือ ช้า ปกติ เร็ว (คอมเมนต์ภาษาจีนของ set_direction) ส่วนที่ Flight Controller ทำกับค่าเหล่านี้ ไม่มีข้อมูล

ขากลับ: อ่านความสูง

ทิศตรงข้าม Arduino เป็นฝ่ายอ่าน

static void iicRecv() {
  const uint8_t FC_ADDR = 0x34;
  const uint8_t LEN = 8;
  Wire.requestFrom(FC_ADDR, LEN);
  // อ่านเข้า m_externData[0..7] เติม 0 ถ้าไม่ครบ
}

uint16_t Arduino::get_height_cm() const {
  return (uint16_t)m_externData[0] | ((uint16_t)m_externData[1] << 8);
}

เรียก drone.poll() ก่อน แล้วค่อย drone.heightCm() ถ้าไม่ poll() จะได้ค่าเก่า ไบต์ [2]–[7] ของ m_externData ไม่มีฟังก์ชันตัวใดอ่านต่อ ความหมายไม่ได้ระบุในโค้ด

ปุ่มจากรีโมท

อีกทิศหนึ่ง receiveEvent เก็บไบต์สุดท้ายที่ได้รับเป็นหมายเลขปุ่ม (m_remoteKey) และ keyDo ใช้เทียบกับ KEY_1…KEY_14 ที่ onKey1…14 ส่งเข้ามา

ลองทำ

  1. เขียนแพ็กเก็ตของ set_direction(4, 3) เป็นเลขฐานสิบหกทั้ง 6 ไบต์ (ไบต์ [0] ใส่ n+1 ได้)
  2. คำนวณไบต์ต่ำ/สูงของ set_height(305) (คำตอบ: 305 = 0x0131 ต่ำ 31 สูง 01)
  3. อ่าน Arduino::set_yaw ใน protocol.cpp แล้วถามตัวเองว่า ทำไมไบต์ [5] ไม่ถูกเขียน
  4. เปรียบเทียบตาราง F_* นี้กับรหัสคำสั่งของรีโมทใน คำสั่งเดินทางยังไง

พังตรงไหนบ่อย

  • คิดว่าเรียกแล้วส่งทันที คำสั่งอยู่ในหน่วยความจำจนกว่า FC จะมาอ่าน ถ้าสั่งสองคำสั่งติดกันโดยไม่รอ คำสั่งแรกถูกเขียนทับก่อนถูกอ่าน เพราะมีกระดานเดียว (นี่คือเหตุผลที่โค้ดชมรมคั่นด้วย delay)
  • ตัวนับกับเนื้อหาไม่ตรงกัน ฟังก์ชันบวกตัวนับก่อนแล้วเขียนไบต์ที่เหลือ ถ้า FC มาอ่านกลางคันในทางทฤษฎีอาจได้ตัวนับใหม่กับรหัสเก่า ยังไม่ปรากฏว่าเกิดจริง ไม่มีหลักฐานสองทาง
  • ใช้ set_pos กับ set_height สลับกัน ใช้ไบต์เดียวกัน
  • เรียก flip() ไม่มีผล
  • ไม่เรียก poll() แล้วอ่านความสูง ได้ค่าเก่าหรือ 0 ที่ยังไม่มีข้อมูล
  • เชื่อว่ารู้ว่า FC ทำอะไรกับเลขพวกนี้ รู้แค่ว่าเราเขียนอะไรลงไป

1. คำสั่งเดินทางยังไง

โปรโตคอลสามชั้นของ RCSA: serial AB CD LEN, I2C 6 ไบต์ และเฟรมฝูงบิน 0xFA พร้อม CRC-16

3. โหมดฝูงบินและ CRC-16

เฟรมที่ขึ้นต้นด้วย 0xFA รหัสคำสั่งฝูง และการอ่านข้อมูลสถานะของโดรนแต่ละลำ จากบันทึกโปรโตคอล RCSA

On this page

หน้าตาของแพ็กเก็ตตัวอย่างทีละคำสั่งตาราง F_*รหัสทิศและความเร็วขากลับ: อ่านความสูงปุ่มจากรีโมทลองทำพังตรงไหนบ่อย