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 