Sorry, I was actually referring to the remote control that he holds in his hands that looks like a standard R/C controller but I presume is a BT remote.
@ ianlee74 - Ah, ok. Itās a USB remote control as normally used with flight simulators only. It is connected to a PC that has Bluetooth, and runs a program that relays the commands.
Well, I was wrong. No source code after the summer breakā¦
No, the code is already here, right now 
Big thanks and kudos to Beat!
Cuno
EDIT Warning, this is code using the [em]original [/em]NETMF 4.2 PWM API. It would also compile under later releases, but would then take the wrong overload of the PWM constructor!
// Copyright 2013 Beat Heeb
// licensed under the Creative Commons
// Attribution-NonCommercial-ShareAlike CC BY-NC-SA license:
// http://creativecommons.org/licenses/by-nc-sa/3.0/legalcode
using Microsoft.SPOT.Hardware;
using System.IO.Ports;
using System.Threading;
class Quadro {
const int sampleFreq = 200; // sample frequency [Hz]
// Bluetooth Serial Connection
static SerialPort serial = new SerialPort(Serial.COM3, 115200);
// SPP_SEND_DATA packet
static byte[] txBuf = { 0x02, 0x52, 0x0F, 25+3, 0, 0x7D, 1, 25, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0x03 };
// reference values from Bluetooth remote control
static int refPower, refRoll, refPitch, refYaw;
// actual battery voltage
static int battery = 3600 << batteryLog; // [mv >> batteryLog]
// quality control
static int errCount, ctrlCount;
// Analog Input Channel (Battery Voltage)
static AnalogInput ad = new AnalogInput(Cpu.AnalogChannel.ANALOG_0); // PC0
// Motor Outputs
static PWM motorF = new PWM(Cpu.PWMChannel.PWM_2, 1024, 0, false); // PC8
static PWM motorR = new PWM(Cpu.PWMChannel.PWM_3, 1024, 0, false); // PC9
static PWM motorB = new PWM(Cpu.PWMChannel.PWM_0, 1024, 0, false); // PC6
static PWM motorL = new PWM(Cpu.PWMChannel.PWM_1, 1024, 0, false); // PC7
// GPIO
static OutputPort led1 = new OutputPort((Cpu.Pin)18, false); // PB2
static OutputPort rts = new OutputPort((Cpu.Pin)30, true); // PB14
static InputPort cts = new InputPort((Cpu.Pin)29, false, Port.ResistorMode.Disabled); // PB13
static InputPort intG = new InputPort((Cpu.Pin)21, false, Port.ResistorMode.PullDown); // PB5
// Acceleration Sensor
static I2CDevice.Configuration adxl345config = new I2CDevice.Configuration(0x53, 400);
static I2CDevice i2c = new I2CDevice(adxl345config);
// BW_RATE: 100Hz, POWER_CTL: Measure
static I2CDevice.I2CWriteTransaction adxl345w1 = I2CDevice.CreateWriteTransaction(new byte[] { 44, 0x0A, 0x08 });
// DATA_FORMAT: +/-4g, right-justified
static I2CDevice.I2CWriteTransaction adxl345w2 = I2CDevice.CreateWriteTransaction(new byte[] { 49, 0x01 });
// DATAX, DATAY, DATAZ
static I2CDevice.I2CWriteTransaction adxl345w3 = I2CDevice.CreateWriteTransaction(new byte[] { 50 });
static byte[] adxl345b2 = new byte[6];
static I2CDevice.I2CReadTransaction adxl345r1 = I2CDevice.CreateReadTransaction(adxl345b2);
static I2CDevice.I2CTransaction[] adxl345t1 = { adxl345w1, adxl345w2 };
static I2CDevice.I2CTransaction[] adxl345t2 = { adxl345w3, adxl345r1 };
const double accSens = 128.0; // accelerometer sensitivity [units/g]
static void InitAdxl345() {
i2c.Config = adxl345config;
int n = i2c.Execute(adxl345t1, 100);
}
static void ReadAdxl345(out int x, out int y, out int z) {
i2c.Config = adxl345config;
int n = i2c.Execute(adxl345t2, 100);
y = (adxl345b2[0] + ((sbyte)adxl345b2[1] << 8));
x = (adxl345b2[2] + ((sbyte)adxl345b2[3] << 8));
z = -(adxl345b2[4] + ((sbyte)adxl345b2[5] << 8));
}
// Giro
static I2CDevice.Configuration itg3200config = new I2CDevice.Configuration(0x68, 400);
// SMPLRT_DIV: 8kHz / n, DLPF_FS: 256Hz, INT_CFG: latched, any reg, data ready
static I2CDevice.I2CWriteTransaction itg3200w1 = I2CDevice.CreateWriteTransaction(new byte[] { 21, 8000 / sampleFreq - 1, 0x18, 0x31 });
// CLK_SEL: gyro_z
static I2CDevice.I2CWriteTransaction itg3200w2 = I2CDevice.CreateWriteTransaction(new byte[] { 62, 0x03 });
// GYRO_XOUT, GYRO_YOUT, GYRO_ZOUT
static I2CDevice.I2CWriteTransaction itg3200w3 = I2CDevice.CreateWriteTransaction(new byte[] { 29 });
static byte[] itg3200b2 = new byte[6];
static I2CDevice.I2CReadTransaction itg3200r1 = I2CDevice.CreateReadTransaction(itg3200b2);
static I2CDevice.I2CTransaction[] itg3200t1 = { itg3200w1, itg3200w2 };
static I2CDevice.I2CTransaction[] itg3200t2 = { itg3200w3, itg3200r1 };
const double gyroSens = 823.6; // gyro sensitivity [units/(rad/s)]
static void InitItg3200() {
i2c.Config = itg3200config;
int n = i2c.Execute(itg3200t1, 100);
}
static void ReadItg3200(out int x, out int y, out int z) {
i2c.Config = itg3200config;
int n = i2c.Execute(itg3200t2, 100);
y = (((sbyte)itg3200b2[0] << 8) + itg3200b2[1]);
x = (((sbyte)itg3200b2[2] << 8) + itg3200b2[3]);
z = -(((sbyte)itg3200b2[4] << 8) + itg3200b2[5]);
}
const int fraction = 24; // fixpoint position (number of fraction bits)
const int one = 1 << fraction; // one in fixpoint representation
const int gyroScale = (int)(one / gyroSens / sampleFreq + 0.5);
const int accScale = (int)(one / accSens + 0.5);
const int calibTime = 5; // calibration time [s]
const int calibSamples = calibTime * sampleFreq; // number of samples averaged for calibration
const int coeffLog = 10; // -log of Kalman gain
const int batteryLog = 6; // -log of battery filter coefficient
const int coeffP = 250; // [mV*s/rad] pitch/roll controller coefficient
const int coeffY = 180; // [mV*s/rad] yaw controller coefficient
const int coeffC = 3; // [1/s] pitch/roll outer controller coefficient
const int coeffTP = 3; // [s] pitch/roll integration time constant
const int coeffTY = 2; // [s] yaw integration time constant
const int prScale = one / 2 / 128; // [rad/unit] pitch/roll reference scale (30Ā° full scale)
const int yScale = one * 3 / 128 / sampleFreq; // [rad/timestep/unit] yaw reference scale (180Ā°/s full scale)
const int tScale = 4096 / 256; // [mv/unit] power (thrust) reference scale (full range)
static int Mul(int x, int y) { // fixpoint multiplication
return (int)((long)x * y >> fraction);
}
static void Main() {
new Thread(new ThreadStart(BTWork)).Start(); // start Bluetooth thread
// initialize sensors and drivers
Thread.Sleep(100);
InitItg3200();
InitAdxl345();
motorF.Start();
motorR.Start();
motorB.Start();
motorL.Start();
// calibrate sensors
int ax, ay, az; // acc sensor values [g] (after scaling)
int gx, gy, gz; // giro values [rad/timestep] (after scaling)
int axs = 0, ays = 0, azs = 0, gxs = 0, gys = 0, gzs = 0; // calibration sums
led1.Write(true); // signal calibration
for (int i = 0; i < calibSamples; i++) {
// wait for sensors ready
while (!intG.Read()) ;
// read sensors
ReadItg3200(out gx, out gy, out gz);
ReadAdxl345(out ax, out ay, out az);
// add up to average
gxs += gx;
gys += gy;
gzs += gz;
axs += ax;
ays += ay;
azs += az;
}
led1.Write(false);
gxs = gxs * gyroScale / calibSamples;
gys = gys * gyroScale / calibSamples;
gzs = gzs * gyroScale / calibSamples;
axs = axs * (accScale >> 4) / (calibSamples >> 4); // avoid overflow
ays = ays * (accScale >> 4) / (calibSamples >> 4);
azs = azs * (accScale >> 4) / (calibSamples >> 4);
// control loop
int dx = 0, dy = 0, dz = one; // down vector (|d| ~= one)
int sumR = 0, sumP = 0, sumY = 0; // integrators
int n = 0;
while (true) {
// wait for sensors ready
while (!intG.Read()) ;
// read and scale sensors (1.20ms)
ReadItg3200(out gx, out gy, out gz);
ReadAdxl345(out ax, out ay, out az);
gx = (gx * gyroScale) - gxs;
gy = (gy * gyroScale) - gys;
gz = (gz * gyroScale) - gzs;
ax = (ax * accScale) - axs;
ay = (ay * accScale) - ays;
az = (az * accScale); // no known offset
// track attitude (0.28ms)
int dx0 = dx, dy0 = dy;
dx = dx + Mul(gz, dy) - Mul(gy, dz); // d + d x g (cross product)
dy = dy + Mul(gx, dz) - Mul(gz, dx0);
dz = dz + Mul(gy, dx0) - Mul(gx, dy0);
// sensor fusion (simple Kalman filter) (0.04ms)
dx -= (dx + ax) >> coeffLog; // gravitation = -acceleration
dy -= (dy + ay) >> coeffLog;
dz -= (dz + az) >> coeffLog;
// attitude control
int aPitch, aRoll, aYaw; // [mv] controller outputs
int aPower = refPower * tScale;
if (refPower > 10) {
// outer (slow) controller (0.11ms)
int errP = refPitch * prScale - dx; // pitch error [rad]
int errR = refRoll * prScale - dy; // roll error [rad]
int errY = refYaw * yScale - gz; // yaw error [rad/timestep]
sumP += errP / (sampleFreq * coeffTP); // pitch integral [rad]
sumR += errR / (sampleFreq * coeffTP); // roll integral [rad]
sumY += errY / coeffTY; // yaw integral [rad/s]
int refP = (errP + sumP) * coeffC; // pitch ref [rad/s]
int refR = (errR + sumR) * coeffC; // roll ref [rad/s]
// inner (fast) controller (0.15ms)
aPitch = Mul(refP + gy * sampleFreq, coeffP); // [mv]
aRoll = Mul(refR - gx * sampleFreq, coeffP); // [mv]
aYaw = Mul(sumY + errY * sampleFreq, coeffY); // [mv]
} else {
aPitch = aRoll = aYaw = 0;
sumR = sumP = sumY = 0;
}
// motor control (0.33ms)
int front = aPower - aPitch + aYaw; // mixer
int right = aPower - aRoll - aYaw;
int back = aPower + aPitch + aYaw;
int left = aPower + aRoll - aYaw;
if (front < 0) front = 0; else if (front > 4096) front = 4096; // saturation
if (right < 0) right = 0; else if (right > 4096) right = 4096;
if (back < 0) back = 0; else if (back > 4096) back = 4096;
if (left < 0) left = 0; else if (left > 4096) left = 4096;
motorF.Duration = (uint)front >> 2; // PWM outputs
motorR.Duration = (uint)right >> 2; // range: 0-1024 = ~0-4V
motorB.Duration = (uint)back >> 2;
motorL.Duration = (uint)left >> 2;
n++;
if ((n & 1) == 0) {
// A/D (battery voltage) (0.10ms)
int res = ad.ReadRaw(); // read battery voltage (u/2 / 3V * 2^12)
res = res + (res >> 1); // convert to mv
battery = battery - (battery >> batteryLog) + res; // low pass filter
// LED blink (0.05ms)
if (n == sampleFreq / 4 - sampleFreq / 20 && battery < 3300 << batteryLog) {
led1.Write(true); // low battery
}
if (n == sampleFreq / 4) {
led1.Write(false);
}
if (n == sampleFreq / 2) {
if (ctrlCount < 2) { // missing control data
led1.Write(true);
refPower = refPower >= 10 ? refPower - 10 : 0; // reduce power
refRoll = refPitch = refYaw = 0;
}
ctrlCount = 0;
}
if (n == sampleFreq - sampleFreq / 20) {
led1.Write(true);
}
if (n == sampleFreq) {
led1.Write(false);
n = 0;
}
} else {
// monitor (0.60ms)
txBuf[9] = (byte)((battery * 100 >> (batteryLog + 10)) - 300); // [V/100], 3V offset
txBuf[10] = (byte)errCount;
txBuf[11] = (byte)gx;
txBuf[12] = (byte)(gx >> 8);
txBuf[13] = (byte)(gx >> 16);
txBuf[14] = (byte)gy;
txBuf[15] = (byte)(gy >> 8);
txBuf[16] = (byte)(gy >> 16);
txBuf[17] = (byte)gz;
txBuf[18] = (byte)(gz >> 8);
txBuf[19] = (byte)(gz >> 16);
txBuf[20] = (byte)(ax >> 10);
txBuf[21] = (byte)(ax >> 18);
txBuf[22] = (byte)(ay >> 10);
txBuf[23] = (byte)(ay >> 18);
txBuf[24] = (byte)(az >> 10);
txBuf[25] = (byte)(az >> 18);
txBuf[26] = (byte)front;
txBuf[27] = (byte)(front >> 8);
txBuf[28] = (byte)right;
txBuf[29] = (byte)(right >> 8);
txBuf[30] = (byte)back;
txBuf[31] = (byte)(back >> 8);
txBuf[32] = (byte)left;
txBuf[33] = (byte)(left >> 8);
serial.Write(txBuf, 0, -1);
}
if (intG.Read()) errCount++; // deadline missed
}
}
// ********** Bluetooth Thread **********
// incoming data packets
// 02 69 10 0900 82 01 0600 557169787BCD 03
static void BTWork() {
byte[] buf = new byte[256];
serial.Open();
rts.Write(false); // activate rts to wake up module
while (true) {
do {
serial.Read(buf, 0, 1); // search for start byte
} while (buf[0] != 0x02);
serial.Read(buf, 1, 5); // read header
serial.Read(buf, 6, buf[3] + 7); // read rest of data
if (buf[1] == 0x69 && buf[2] == 0x10) { // incoming data packet
if (buf[9] == 0x55) { // remote control values
refPower = buf[10];
refRoll = buf[11] - 128;
refPitch = buf[12] - 128;
refYaw = buf[13] - 128;
ctrlCount++;
}
}
}
}
}
5 Likes
@ Cuno - Awesome! Thanks. The wife & kids are going out of town for the weekend. Iām going to revive my quadcopter project!
Indeed! This is great! Thanks to both of you! I would put it on codeshare though. Here it can get lost easily. 
Thanks for sharing!
@ Cuno Thanks for sharing! I have one question, does this code work with NETMF, or does it need your implementation of NETMF? as described in the first post
@ Davef - I donĀ“t think I understand your question. Our [em]NETMF for STM32 [/em]is simply a port of MicrosoftĀ“s .NET Micro Framework (NETMF) for specific hardware. Thus it [em]is [/em]NETMF (as we implemented it for ARMĀ“s Cortex-M core in general, and STM32 microcontrollers in particular). We have strictly avoided non-standard features (no own APIs, except for the necessarily hardware-specific hardware provider for the Mountaineer mainboards). So it should run on any good NETMF implementation.
I donĀ“t know whether all NETMF ports for Cortex-M microcontrollers are derivatives of [em]NETMF for STM32[/em]. I guess that at least the tricky Cortex-M core initializations are used in all Cortex-M ports today. At least the firmware of FEZ Cerberus, Netduino Go and similar STM32-based boards definitely are derivatives of our code. If the device drivers have not been modified, these ports should thus behave in a very similar way.
Or maybe you meant the remark regarding NETMF 4.2 versus later Releases (i.e., 4.2 QFE1, 4.2 QFE2, and 4.3)? Release 4.2 introduced a PWM API. This is what Beat used for his quadrocopter. In 4.2 QFE1, this API was modified in a somewhat unfortunate way, thus the warning.
@ Cuno - Sorry, confusing people appears to be my M.Oā¦ My comments were based solely on your first post where you talk about changing interrupt priorities, putting GC in itās (in my opinion) rightful place, with a low priority! Itās clear now, that what is being used is standard. Thanks for the reply.
P.S. If it had been non-standard NETMF i would have definitely wanted a copy!
Hi Cuno:
How do you know the exact time interval?
for example in your code,
// read and scale sensors (1.20ms)
@ Tzu, because I bet one device sets the pin it reads at that frequency.
But he know the timing of every task?
// track attitude (0.28ms)
// sensor fusion (simple Kalman filter) (0.04ms)
// outer (slow) controller (0.11ms)
@ Tzu Hsuan - well the Swiss are known for everything running like clockwork :whistle:
@ Justin - Haha
Ha ha, I donāt understand Justinās Joke.
Do you answer my question?
Iām suprised by the announced 200Hz cycle frquency.
The NETMF scheduler have a 20ms time slice. Each of the 2 threads is then running for 20ms alternatively.
This can be tweaked by threads priority but this not reduce the time slice, a thread can consume 2 or more slices at its turn. A thread can also call Thread.Sleep(0) to switch to next thread before the scheduler normal switch. I canāt find this in the code.
It seems that 200Hz is achieved only half the time.
Did I miss something?
Here itās busy waiting, not thread scheduling.
I was talking about the bluetooth thread.
Finally, I think the key is that SerialPort.Read() does not keep its thread running. It is not polling but interrupt driven.
Hi!
Nice work, really. The source code looks a little bit difficult to understand because Iām not familiar with hardware.
What about the components? You bought a few things, no?
Iām thinking to build a quadrocopter using NETMF with Netduino Plus 2. is it a good idea to use this one? (STMicro STM32F4 164 Mhz (Cortex-M4))
And about the price? Itās for a school project with 4 other students.
Thank you 