sieni
/
luckfox-pico
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update webrtc + lucfox_ctrl

This commit is contained in:
phamvannhat 2025-12-13 11:59:29 +07:00
parent 6587b867d4
commit 14cfc38d89
5 changed files with 591 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
app/driver/luckfox_ctrl/Makefile Normal file
View File

@ -0,0 +1,13 @@
obj-m += luckfox_ctrl.o
KDIR := /home/phamvannhat/luckfox-pico/sysdrv/source/kernel
ARCH := arm
CROSS_COMPILE := arm-rockchip830-linux-uclibcgnueabihf-
JOBS ?= $(shell nproc)
all:
$(MAKE) -C $(KDIR) M=$(PWD) ARCH=$(ARCH) CROSS_COMPILE=$(CROSS_COMPILE) -j$(JOBS) modules
clean:
$(MAKE) -C $(KDIR) M=$(PWD) clean

267
app/driver/luckfox_ctrl/luckfox_ctrl.c Normal file
View File

@ -0,0 +1,267 @@
/* luckfox_ctrl.c - ioctl servo (MG90S mapping) supporting 4-cell DT pwms
*
* - Uses devm_of_pwm_get() so DT entries like:
* pwms = <&pwm0 0 20000000 0>;
* are supported.
*
* - Uses same mapping as original pwm_servo.c:
* DUTY_RIGHT_X100 = 47.02% (x100)
* DUTY_LEFT_X100 = 70.76% (x100)
* SERVO_MAX_DEG = 120
* SERVO_PERIOD_NS = 2515000 (kept from original mapping)
*
* - Exposes /dev/luckfox_ctrl and an ioctl:
* SERVO_SET_ANGLE (int)
*
* Build as kernel module (obj-m += luckfox_ctrl.o)
*/
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/fs.h>
#include <linux/uaccess.h>
#include <linux/device.h>
#include <linux/cdev.h>
#include <linux/of.h>
#include <linux/of_platform.h>
#include <linux/pwm.h>
#include <linux/gpio/consumer.h>
#include <linux/mutex.h>
#include <linux/platform_device.h>
#define DEVICE_NAME "luckfox_ctrl"
/* IOCTLs */
#define SERVO_SET_ANGLE _IOW('p', 1, int)
#define GPIO_SET_LEVEL _IOW('p', 2, int)
#define GPIO_SET_DIR _IOW('p', 3, int)
/* MG90S mapping (from your pwm_servo.c) */
#define SERVO_MAX_DEG 120U
#define SERVO_PERIOD_NS 2515000U /* 2.515 ms */
#define DUTY_RIGHT_X100 4702U /* 47.02% x100 */
#define DUTY_LEFT_X100 7076U /* 70.76% x100 */
struct luckfox_priv {
struct pwm_device *servo_pwm;
struct gpio_desc *gpio_test;
struct mutex lock;
dev_t devno;
struct cdev cdev;
struct class *class;
};
static int servo_angle_to_pulse_ns(unsigned int deg, u32 *out_pulse_ns)
{
u32 duty_x100;
u32 pulse_ns;
if (deg > SERVO_MAX_DEG)
deg = SERVO_MAX_DEG;
duty_x100 = DUTY_RIGHT_X100 +
((DUTY_LEFT_X100 - DUTY_RIGHT_X100) * deg) / SERVO_MAX_DEG;
/* pulse = period * duty_x100 / 10000 */
pulse_ns = (SERVO_PERIOD_NS / 10000U) * duty_x100;
*out_pulse_ns = pulse_ns;
return 0;
}
static int servo_write_angle(struct luckfox_priv *p, unsigned int angle)
{
u32 pulse_ns;
if (!p || !p->servo_pwm)
return -ENODEV;
servo_angle_to_pulse_ns(angle, &pulse_ns);
/* configure pwm (use same period as original mapping) */
pwm_config(p->servo_pwm, pulse_ns, SERVO_PERIOD_NS);
pwm_enable(p->servo_pwm);
return 0;
}
/* ioctl handler */
static long luckfox_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
struct luckfox_priv *p = file->private_data;
int val;
if (!p)
return -ENODEV;
switch (cmd) {
case SERVO_SET_ANGLE:
if (copy_from_user(&val, (void __user *)arg, sizeof(int)))
return -EFAULT;
if (val < 0) val = 0;
if (val > (int)SERVO_MAX_DEG) val = SERVO_MAX_DEG;
mutex_lock(&p->lock);
servo_write_angle(p, (unsigned int)val);
mutex_unlock(&p->lock);
pr_info("luckfox_ctrl: set angle=%d\n", val);
break;
case GPIO_SET_LEVEL:
if (!p->gpio_test) return -ENODEV;
if (copy_from_user(&val, (void __user *)arg, sizeof(int)))
return -EFAULT;
gpiod_set_value(p->gpio_test, val ? 1 : 0);
pr_info("luckfox_ctrl: gpio set level=%d\n", val);
break;
case GPIO_SET_DIR:
if (!p->gpio_test) return -ENODEV;
if (copy_from_user(&val, (void __user *)arg, sizeof(int)))
return -EFAULT;
gpiod_direction_output(p->gpio_test, val ? 1 : 0);
pr_info("luckfox_ctrl: gpio set dir out (init=%d)\n", val);
break;
default:
return -EINVAL;
}
return 0;
}
static int luckfox_open(struct inode *inode, struct file *filp)
{
struct luckfox_priv *p = container_of(inode->i_cdev, struct luckfox_priv, cdev);
filp->private_data = p;
return 0;
}
static int luckfox_release(struct inode *inode, struct file *filp)
{
filp->private_data = NULL;
return 0;
}
static const struct file_operations fops = {
.owner = THIS_MODULE,
.unlocked_ioctl = luckfox_ioctl,
.open = luckfox_open,
.release = luckfox_release,
};
/* probe */
static int luckfox_ctrl_probe(struct platform_device *pdev)
{
struct device *dev = &pdev->dev;
struct luckfox_priv *p;
int ret;
pr_info("luckfox_ctrl: probe\n");
p = devm_kzalloc(dev, sizeof(*p), GFP_KERNEL);
if (!p)
return -ENOMEM;
mutex_init(&p->lock);
/* Get PWM from DT by name "mg90s" (supports 4-cell pwms in DT) */
p->servo_pwm = devm_of_pwm_get(dev, dev->of_node, "mg90s");
if (IS_ERR(p->servo_pwm)) {
dev_err(dev, "cannot get servo PWM (mg90s): %ld\n", PTR_ERR(p->servo_pwm));
return PTR_ERR(p->servo_pwm);
}
/* Optional GPIO named "test" in DT (optional) */
p->gpio_test = devm_gpiod_get_optional(dev, "test", GPIOD_OUT_LOW);
if (IS_ERR(p->gpio_test)) {
dev_err(dev, "failed to get optional test gpio\n");
/* not fatal: continue without gpio */
p->gpio_test = NULL;
}
/* Allocate char device number */
ret = alloc_chrdev_region(&p->devno, 0, 1, DEVICE_NAME);
if (ret) {
dev_err(dev, "alloc_chrdev_region failed: %d\n", ret);
return ret;
}
/* init cdev as part of priv so we can find priv from inode */
cdev_init(&p->cdev, &fops);
p->cdev.owner = THIS_MODULE;
ret = cdev_add(&p->cdev, p->devno, 1);
if (ret) {
dev_err(dev, "cdev_add failed: %d\n", ret);
unregister_chrdev_region(p->devno, 1);
return ret;
}
p->class = class_create(THIS_MODULE, DEVICE_NAME);
if (IS_ERR(p->class)) {
dev_err(dev, "class_create failed\n");
cdev_del(&p->cdev);
unregister_chrdev_region(p->devno, 1);
return PTR_ERR(p->class);
}
if (!device_create(p->class, NULL, p->devno, NULL, DEVICE_NAME)) {
dev_err(dev, "device_create failed\n");
class_destroy(p->class);
cdev_del(&p->cdev);
unregister_chrdev_region(p->devno, 1);
return -ENOMEM;
}
/* store priv in platform device */
platform_set_drvdata(pdev, p);
pr_info("luckfox_ctrl: ready at /dev/%s (major=%d minor=%d)\n",
DEVICE_NAME, MAJOR(p->devno), MINOR(p->devno));
/* set default 0 angle to be safe */
mutex_lock(&p->lock);
servo_write_angle(p, 0);
mutex_unlock(&p->lock);
return 0;
}
static int luckfox_ctrl_remove(struct platform_device *pdev)
{
struct luckfox_priv *p = platform_get_drvdata(pdev);
if (!p) return 0;
pwm_disable(p->servo_pwm);
device_destroy(p->class, p->devno);
class_destroy(p->class);
cdev_del(&p->cdev);
unregister_chrdev_region(p->devno, 1);
pr_info("luckfox_ctrl removed\n");
return 0;
}
static const struct of_device_id luckfox_dt_ids[] = {
{ .compatible = "luckfox,ctrl" },
{ }
};
MODULE_DEVICE_TABLE(of, luckfox_dt_ids);
static struct platform_driver luckfox_ctrl_driver = {
.probe = luckfox_ctrl_probe,
.remove = luckfox_ctrl_remove,
.driver = {
.name = "luckfox_ctrl",
.of_match_table = luckfox_dt_ids,
},
};
module_platform_driver(luckfox_ctrl_driver);
MODULE_LICENSE("GPL");
MODULE_AUTHOR("ChatGPT");
MODULE_DESCRIPTION("Luckfox control driver (MG90S ioctl) - supports 4-cell DT pwms");

289
app/libdc_udp_demo/src/peer_client.cpp
View File

@ -3,144 +3,273 @@
#include <thread>
#include <atomic>
#include <vector>
#include <cstring>
#include <algorithm>
#include "udp_signaling.hpp"
#include "rtc/rtc.hpp"
// ---------- base64 helpers ----------
static const std::string b64_table = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
//--------------------------------------------------
// ENUM COMMAND
//--------------------------------------------------
typedef enum {
CMD_NONE = 0x00,
CMD_PING = 0x01,
CMD_GET_STATUS = 0x02,
CMD_GET_SENSOR = 0x03,
CMD_SET_SPEED = 0x10,
CMD_SET_DIRECTION = 0x11,
CMD_SET_STEERING = 0x12,
CMD_SET_BRAKE = 0x13,
CMD_START_MOTOR = 0x20,
CMD_STOP_MOTOR = 0x21,
CMD_MAX
} Command_t;
// ACK
typedef enum {
ACK_OK = 0x00,
ACK_INVALID_CMD = 0x01,
ACK_INVALID_PAYLOAD = 0x02,
ACK_CRC_ERROR = 0x03,
ACK_BUSY = 0x04,
ACK_FAIL = 0x05
} AckCode_t;
//--------------------------------------------------
// Frame Struct
//--------------------------------------------------
typedef struct {
uint8_t sof;
uint8_t msg_id;
uint8_t cmd;
uint8_t length;
uint8_t payload[256];
uint8_t checksum;
} Frame_t;
//--------------------------------------------------
// Checksum
//--------------------------------------------------
uint8_t calc_checksum(const Frame_t& f) {
uint8_t sum = f.sof ^ f.msg_id ^ f.cmd ^ f.length;
for (int i = 0; i < f.length; i++)
sum ^= f.payload[i];
return sum;
}
//--------------------------------------------------
// Build ACK frame
//--------------------------------------------------
std::vector<uint8_t> build_frame(uint8_t msg_id, uint8_t cmd, const uint8_t* payload, uint8_t len) {
Frame_t f{};
f.sof = 0xAA;
f.msg_id = msg_id;
f.cmd = cmd;
f.length = len;
if (len > 0 && payload != nullptr)
memcpy(f.payload, payload, len);
f.checksum = calc_checksum(f);
std::vector<uint8_t> out;
out.reserve(4 + len + 1);
out.push_back(f.sof);
out.push_back(f.msg_id);
out.push_back(f.cmd);
out.push_back(f.length);
for (int i = 0; i < len; i++)
out.push_back(f.payload[i]);
out.push_back(f.checksum);
return out;
}
//--------------------------------------------------
// Parse frame
//--------------------------------------------------
bool parse_frame(const uint8_t* buf, size_t size, Frame_t& out) {
if (size < 5) return false;
if (buf[0] != 0xAA) return false;
out.sof = buf[0];
out.msg_id = buf[1];
out.cmd = buf[2];
out.length = buf[3];
if (4 + out.length + 1 != size)
return false;
memcpy(out.payload, &buf[4], out.length);
out.checksum = buf[4 + out.length];
return out.checksum == calc_checksum(out);
}
//--------------------------------------------------
// Base64
//--------------------------------------------------
static const std::string b64_table =
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
static std::string base64_encode(const std::string &in) {
std::string out;
int val = 0, valb = -6;
for (unsigned char c : in) {
val = (val<<8) + c;
val = (val << 8) + c;
valb += 8;
while (valb >= 0) {
out.push_back(b64_table[(val>>valb)&0x3F]);
out.push_back(b64_table[(val >> valb) & 0x3F]);
valb -= 6;
}
}
if (valb>-6) out.push_back(b64_table[((val<<8)>>(valb+8))&0x3F]);
while (out.size()%4) out.push_back('=');
if (valb > -6)
out.push_back(b64_table[((val << 8) >> (valb + 8)) & 0x3F]);
while (out.size() % 4)
out.push_back('=');
return out;
}
static std::string base64_decode(const std::string &in) {
std::vector<int> T(256,-1);
for (int i=0;i<64;i++) T[(unsigned char)b64_table[i]] = i;
std::vector<int> T(256, -1);
for (int i = 0; i < 64; i++)
T[(unsigned char)b64_table[i]] = i;
std::string out;
int val=0, valb=-8;
int val = 0, valb = -8;
for (unsigned char c : in) {
if (T[c] == -1) break;
val = (val<<6) + T[c];
if (T[c] == -1)
break;
val = (val << 6) + T[c];
valb += 6;
if (valb >= 0) {
out.push_back(char((val>>valb)&0xFF));
out.push_back(char((val >> valb) & 0xFF));
valb -= 8;
}
}
return out;
}
//--------------------------------------------------
// Helper: convert vector<uint8_t> -> rtc::binary (vector<std::byte>)
//--------------------------------------------------
static rtc::binary vec_u8_to_rtcbin(const std::vector<uint8_t>& v) {
rtc::binary out(v.size());
for (size_t i = 0; i < v.size(); ++i) out[i] = std::byte(v[i]);
return out;
}
//--------------------------------------------------
// MAIN SERVER
//--------------------------------------------------
int main() {
const int SERVER_PORT = 6000;
std::cout << "[Server] Starting UDP signaling on port " << SERVER_PORT << std::endl;
// server: local_ip empty -> bind INADDR_ANY
std::cout << "[Server] UDP Signaling on port " << SERVER_PORT << std::endl;
UdpSignaling signaling("", SERVER_PORT);
signaling.start();
rtc::Configuration config;
// STUN optional when using Tailscale; harmless to keep
config.iceServers.emplace_back("stun:stun.l.google.com:19302");
auto pc = std::make_shared<rtc::PeerConnection>(config);
pc->onStateChange([](rtc::PeerConnection::State s) {
std::cout << "[Server] PC state: " << (int)s << std::endl;
});
// send local SDP via UDP when created (base64)
// Send local SDP
pc->onLocalDescription([&signaling](rtc::Description desc) {
std::string sdp = std::string(desc);
std::string msg = "SDP|" + base64_encode(sdp);
std::string msg = "SDP|" + base64_encode(std::string(desc));
signaling.send(msg);
std::cout << "[Server] Sent local SDP (" << msg.size() << " bytes)\n";
std::cout << "[Server] Sent SDP\n";
});
// send local ICE
// Send ICE
pc->onLocalCandidate([&signaling](rtc::Candidate cand) {
std::string payload = cand.candidate();
std::string mid = cand.mid();
std::string msg = "ICE|" + base64_encode(payload) + "|" + base64_encode(mid);
std::string msg = "ICE|" + base64_encode(cand.candidate())
+ "|" + base64_encode(cand.mid());
signaling.send(msg);
std::cout << "[Server] Sent ICE candidate\n";
std::cout << "[Server] Sent ICE\n";
});
std::shared_ptr<rtc::DataChannel> dc_remote;
// when remote creates a DataChannel, set callbacks
pc->onDataChannel([&dc_remote](std::shared_ptr<rtc::DataChannel> dc) {
std::cout << "[Server] onDataChannel label=" << dc->label() << std::endl;
//--------------------------------------------------
// DataChannel RECEIVED from client
//--------------------------------------------------
pc->onDataChannel([&](std::shared_ptr<rtc::DataChannel> dc) {
std::cout << "[Server] DataChannel opened: " << dc->label() << "\n";
dc_remote = dc;
dc->onOpen([dc](){ std::cout << "[Server] DataChannel opened\n"; });
dc->onMessage([dc](rtc::message_variant msg){
if (std::holds_alternative<rtc::string>(msg)) {
std::string s = std::get<rtc::string>(msg);
std::cout << "[Server] Received from client: " << s << std::endl;
// When client sends a frame
dc->onMessage([dc](rtc::message_variant msg) {
if (!std::holds_alternative<rtc::binary>(msg)) {
std::cout << "[Server] Non-binary message ignored\n";
return;
}
auto bin = std::get<rtc::binary>(msg);
Frame_t f{};
bool ok = parse_frame(
reinterpret_cast<const uint8_t*>(bin.data()),
bin.size(),
f
);
if (!ok) {
std::cout << "[Server] Invalid frame CRC\n";
return;
}
std::cout << "[Server] Received CMD=" << (int)f.cmd
<< " msg_id=" << (int)f.msg_id
<< " len=" << (int)f.length << "\n";
//--------------------------------------------------
// Build ACK and send back
//--------------------------------------------------
uint8_t ack_payload[1] = { ACK_OK };
auto ack = build_frame(f.msg_id, f.cmd, ack_payload, 1);
// convert to rtc::binary and send
rtc::binary bin_ack = vec_u8_to_rtcbin(ack);
try {
dc->send(bin_ack);
std::cout << "[Server] Sent ACK for cmd=" << (int)f.cmd << "\n";
} catch (const std::exception &e) {
std::cout << "[Server] ACK send exception: " << e.what() << "\n";
} catch (...) {
std::cout << "[Server] ACK send unknown exception\n";
}
});
});
// signaling receive handler (from client). server will learn client ip on first packet automatically
signaling.onMessage([&](const std::string &msg, const std::string &src_ip, int src_port){
std::cout << "[Server] Signaling msg from " << src_ip << ":" << src_port << " (" << msg.size() << " bytes)\n";
// set remote explicitly (optional) so future sends use that endpoint
signaling.setRemote(src_ip, src_port);
//--------------------------------------------------
// Signaling via UDP
//--------------------------------------------------
signaling.onMessage([&](const std::string &msg, const std::string &ip, int port) {
signaling.setRemote(ip, port);
if (msg.rfind("SDP|", 0) == 0) {
std::string b = msg.substr(4);
std::string sdp = base64_decode(b);
std::string sdp = base64_decode(msg.substr(4));
pc->setRemoteDescription(rtc::Description(sdp));
pc->setLocalDescription(); // create answer
std::cout << "[Server] Set remote SDP and created local answer\n";
} else if (msg.rfind("ICE|", 0) == 0) {
size_t p1 = msg.find('|', 4);
if (p1 != std::string::npos) {
std::string b1 = msg.substr(4, p1-4);
std::string b2 = msg.substr(p1+1);
std::string candidate = base64_decode(b1);
std::string mid = base64_decode(b2);
pc->addRemoteCandidate(rtc::Candidate(candidate, mid));
std::cout << "[Server] Added remote ICE candidate\n";
}
} else {
std::cout << "[Server] Unknown signaling msg\n";
pc->setLocalDescription(); // generate answer
}
});
// background thread: periodically send control commands when DC open
std::atomic<bool> running{true};
std::thread sender([&](){
while (running) {
std::this_thread::sleep_for(std::chrono::seconds(2));
if (dc_remote && dc_remote->isOpen()) {
std::string cmd = R"({"cmd":"PING","ts":)" + std::to_string(time(nullptr)) + "}";
try {
dc_remote->send(cmd);
std::cout << "[Server] Sent command: " << cmd << std::endl;
} catch (...) {
std::cout << "[Server] send failed\n";
}
else if (msg.rfind("ICE|", 0) == 0) {
size_t p = msg.find('|', 4);
if (p != std::string::npos) {
std::string c = base64_decode(msg.substr(4, p-4));
std::string mid = base64_decode(msg.substr(p+1));
pc->addRemoteCandidate(rtc::Candidate(c, mid));
}
}
});
std::cout << "[Server] Ready. Waiting for client signaling..." << std::endl;
std::cout << "[Server] Ready. Waiting for WebRTC client...\n";
// keep main alive
while (true) std::this_thread::sleep_for(std::chrono::seconds(1));
running = false;
sender.join();
signaling.stop();
return 0;
while (1) std::this_thread::sleep_for(std::chrono::seconds(1));
}

206
app/libdc_udp_demo_luckfox/src/peer_client.cpp
View File

@ -1,16 +1,21 @@
#include <iostream>
#include <memory>
#include <thread>
#include <atomic>
#include <vector>
#include <cstring>
#include <algorithm>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include "udp_signaling.hpp"
#include "rtc/rtc.hpp"
//--------------------------------------------------
// ENUM COMMAND
//--------------------------------------------------
/* ================= IOCTL ================= */
#define SERVO_SET_ANGLE _IOW('p', 1, int)
#define GPIO_SET_LEVEL _IOW('p', 2, int)
/* ================= COMMAND ENUM ================= */
typedef enum {
CMD_NONE = 0x00,
CMD_PING = 0x01,
@ -24,11 +29,9 @@ typedef enum {
CMD_START_MOTOR = 0x20,
CMD_STOP_MOTOR = 0x21,
CMD_MAX
} Command_t;
// ACK
/* ================= ACK ENUM ================= */
typedef enum {
ACK_OK = 0x00,
ACK_INVALID_CMD = 0x01,
@ -38,9 +41,7 @@ typedef enum {
ACK_FAIL = 0x05
} AckCode_t;
//--------------------------------------------------
// Frame Struct
//--------------------------------------------------
/* ================= FRAME ================= */
typedef struct {
uint8_t sof;
uint8_t msg_id;
@ -50,9 +51,7 @@ typedef struct {
uint8_t checksum;
} Frame_t;
//--------------------------------------------------
// Checksum
//--------------------------------------------------
/* ================= CHECKSUM ================= */
uint8_t calc_checksum(const Frame_t& f) {
uint8_t sum = f.sof ^ f.msg_id ^ f.cmd ^ f.length;
for (int i = 0; i < f.length; i++)
@ -60,23 +59,22 @@ uint8_t calc_checksum(const Frame_t& f) {
return sum;
}
//--------------------------------------------------
// Build ACK frame
//--------------------------------------------------
std::vector<uint8_t> build_frame(uint8_t msg_id, uint8_t cmd, const uint8_t* payload, uint8_t len) {
/* ================= BUILD FRAME ================= */
std::vector<uint8_t> build_frame(uint8_t msg_id, uint8_t cmd,
const uint8_t* payload, uint8_t len) {
Frame_t f{};
f.sof = 0xAA;
f.msg_id = msg_id;
f.cmd = cmd;
f.length = len;
if (len > 0 && payload != nullptr)
if (len && payload)
memcpy(f.payload, payload, len);
f.checksum = calc_checksum(f);
std::vector<uint8_t> out;
out.reserve(4 + len + 1);
out.reserve(5 + len);
out.push_back(f.sof);
out.push_back(f.msg_id);
out.push_back(f.cmd);
@ -88,19 +86,17 @@ std::vector<uint8_t> build_frame(uint8_t msg_id, uint8_t cmd, const uint8_t* pay
return out;
}
//--------------------------------------------------
// Parse frame
//--------------------------------------------------
/* ================= PARSE FRAME ================= */
bool parse_frame(const uint8_t* buf, size_t size, Frame_t& out) {
if (size < 5) return false;
if (buf[0] != 0xAA) return false;
out.sof = buf[0];
out.sof = buf[0];
out.msg_id = buf[1];
out.cmd = buf[2];
out.cmd = buf[2];
out.length = buf[3];
if (4 + out.length + 1 != size)
if (size != (size_t)(5 + out.length - 0))
return false;
memcpy(out.payload, &buf[4], out.length);
@ -109,13 +105,11 @@ bool parse_frame(const uint8_t* buf, size_t size, Frame_t& out) {
return out.checksum == calc_checksum(out);
}
//--------------------------------------------------
// Base64
//--------------------------------------------------
/* ================= BASE64 ================= */
static const std::string b64_table =
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
static std::string base64_encode(const std::string &in) {
std::string base64_encode(const std::string &in) {
std::string out;
int val = 0, valb = -6;
for (unsigned char c : in) {
@ -133,20 +127,17 @@ static std::string base64_encode(const std::string &in) {
return out;
}
static std::string base64_decode(const std::string &in) {
std::string base64_decode(const std::string &in) {
std::vector<int> T(256, -1);
for (int i = 0; i < 64; i++)
T[(unsigned char)b64_table[i]] = i;
std::string out;
int val = 0, valb = -8;
for (unsigned char c : in) {
if (T[c] == -1)
break;
if (T[c] == -1) break;
val = (val << 6) + T[c];
valb += 6;
if (valb >= 0) {
out.push_back(char((val >> valb) & 0xFF));
valb -= 8;
@ -155,22 +146,25 @@ static std::string base64_decode(const std::string &in) {
return out;
}
//--------------------------------------------------
// Helper: convert vector<uint8_t> -> rtc::binary (vector<std::byte>)
//--------------------------------------------------
/* ================= HELPER ================= */
static rtc::binary vec_u8_to_rtcbin(const std::vector<uint8_t>& v) {
rtc::binary out(v.size());
for (size_t i = 0; i < v.size(); ++i) out[i] = std::byte(v[i]);
for (size_t i = 0; i < v.size(); i++)
out[i] = std::byte(v[i]);
return out;
}
//--------------------------------------------------
// MAIN SERVER
//--------------------------------------------------
/* ================= MAIN ================= */
int main() {
const int SERVER_PORT = 6000;
std::cout << "[Server] UDP Signaling on port " << SERVER_PORT << std::endl;
int ctrl_fd = open("/dev/luckfox_ctrl", O_RDWR);
if (ctrl_fd < 0) {
perror("open /dev/luckfox_ctrl");
return -1;
}
std::cout << "[Server] UDP signaling on port " << SERVER_PORT << "\n";
UdpSignaling signaling("", SERVER_PORT);
signaling.start();
@ -180,97 +174,91 @@ int main() {
auto pc = std::make_shared<rtc::PeerConnection>(config);
// Send local SDP
pc->onLocalDescription([&signaling](rtc::Description desc) {
std::string msg = "SDP|" + base64_encode(std::string(desc));
signaling.send(msg);
std::cout << "[Server] Sent SDP\n";
pc->onLocalDescription([&](rtc::Description desc) {
signaling.send("SDP|" + base64_encode(std::string(desc)));
});
// Send ICE
pc->onLocalCandidate([&signaling](rtc::Candidate cand) {
std::string msg = "ICE|" + base64_encode(cand.candidate())
+ "|" + base64_encode(cand.mid());
signaling.send(msg);
std::cout << "[Server] Sent ICE\n";
pc->onLocalCandidate([&](rtc::Candidate cand) {
signaling.send("ICE|" + base64_encode(cand.candidate()) +
"|" + base64_encode(cand.mid()));
});
std::shared_ptr<rtc::DataChannel> dc_remote;
//--------------------------------------------------
// DataChannel RECEIVED from client
//--------------------------------------------------
pc->onDataChannel([&](std::shared_ptr<rtc::DataChannel> dc) {
std::cout << "[Server] DataChannel opened: " << dc->label() << "\n";
dc_remote = dc;
std::cout << "[Server] DataChannel opened\n";
// When client sends a frame
dc->onMessage([dc](rtc::message_variant msg) {
if (!std::holds_alternative<rtc::binary>(msg)) {
std::cout << "[Server] Non-binary message ignored\n";
dc->onMessage([dc, ctrl_fd](rtc::message_variant msg) {
if (!std::holds_alternative<rtc::binary>(msg))
return;
}
auto bin = std::get<rtc::binary>(msg);
Frame_t f{};
bool ok = parse_frame(
reinterpret_cast<const uint8_t*>(bin.data()),
bin.size(),
f
);
if (!ok) {
std::cout << "[Server] Invalid frame CRC\n";
return;
uint8_t ack = ACK_OK;
if (!parse_frame(reinterpret_cast<const uint8_t*>(bin.data()),
bin.size(), f)) {
ack = ACK_CRC_ERROR;
} else {
switch (f.cmd) {
case CMD_SET_STEERING: {
if (f.length < 1) {
ack = ACK_INVALID_PAYLOAD;
break;
}
int angle = std::clamp((int)f.payload[0], 0, 120);
if (ioctl(ctrl_fd, SERVO_SET_ANGLE, &angle) < 0) {
perror("SERVO_SET_ANGLE");
ack = ACK_FAIL;
}
break;
}
case CMD_START_MOTOR: {
int v = 1;
if (ioctl(ctrl_fd, GPIO_SET_LEVEL, &v) < 0)
ack = ACK_FAIL;
break;
}
case CMD_STOP_MOTOR: {
int v = 0;
if (ioctl(ctrl_fd, GPIO_SET_LEVEL, &v) < 0)
ack = ACK_FAIL;
break;
}
default:
ack = ACK_INVALID_CMD;
break;
}
}
std::cout << "[Server] Received CMD=" << (int)f.cmd
<< " msg_id=" << (int)f.msg_id
<< " len=" << (int)f.length << "\n";
//--------------------------------------------------
// Build ACK and send back
//--------------------------------------------------
uint8_t ack_payload[1] = { ACK_OK };
auto ack = build_frame(f.msg_id, f.cmd, ack_payload, 1);
// convert to rtc::binary and send
rtc::binary bin_ack = vec_u8_to_rtcbin(ack);
try {
dc->send(bin_ack);
std::cout << "[Server] Sent ACK for cmd=" << (int)f.cmd << "\n";
} catch (const std::exception &e) {
std::cout << "[Server] ACK send exception: " << e.what() << "\n";
} catch (...) {
std::cout << "[Server] ACK send unknown exception\n";
}
auto ack_frame = build_frame(f.msg_id, f.cmd, &ack, 1);
dc->send(vec_u8_to_rtcbin(ack_frame));
});
});
//--------------------------------------------------
// Signaling via UDP
//--------------------------------------------------
signaling.onMessage([&](const std::string &msg, const std::string &ip, int port) {
signaling.onMessage([&](const std::string &msg,
const std::string &ip, int port) {
signaling.setRemote(ip, port);
if (msg.rfind("SDP|", 0) == 0) {
std::string sdp = base64_decode(msg.substr(4));
pc->setRemoteDescription(rtc::Description(sdp));
pc->setLocalDescription(); // generate answer
pc->setRemoteDescription(
rtc::Description(base64_decode(msg.substr(4))));
pc->setLocalDescription();
}
else if (msg.rfind("ICE|", 0) == 0) {
size_t p = msg.find('|', 4);
if (p != std::string::npos) {
std::string c = base64_decode(msg.substr(4, p-4));
std::string mid = base64_decode(msg.substr(p+1));
pc->addRemoteCandidate(rtc::Candidate(c, mid));
}
pc->addRemoteCandidate(
rtc::Candidate(
base64_decode(msg.substr(4, p - 4)),
base64_decode(msg.substr(p + 1))));
}
});
std::cout << "[Server] Ready. Waiting for WebRTC client...\n";
std::cout << "[Server] Ready\n";
while (1) std::this_thread::sleep_for(std::chrono::seconds(1));
}

9
sysdrv/source/kernel/arch/arm/boot/dts/rv1106g-luckfox-pico-zero.dts
View File

@ -34,10 +34,11 @@
status = "okay";
};
mg90s_servo: mg90s-servo {
compatible = "pwm-servo";
pwms = <&pwm0 0 20000000 0>; /* pwm0 channel 0, 20ms, polarity normal */
pwm-names = "mg90s"; /* <-- MATCH with driver */
luckfox_ctrl: luckfox-ctrl {
compatible = "luckfox,ctrl";
pwms = <&pwm0 0 20000000 0>;
pwm-names = "mg90s";
test-gpios = <&gpio1 1 GPIO_ACTIVE_HIGH>;
status = "okay";
};
};