标签：
四旋翼的整个代码是在DAVE写的，DAVE是一个基于Eclipse的IDE，它包含一个标准C编码环境和一些预定义的文件。
LARIX板是四旋翼的主要部分。它收集输入数据，如从RC收到指定的姿态数据以及从IMU收到的实际姿态数据，然后通过数字控制器处理这些数据，再输出BLDC电机需要的PWM信号给PINUS板子。
四旋翼的姿态控制代码，在AttitudeController.c 和 AttitudeController.h中。
这个函数负责确定控制器的输出信号，基于IMU和RC的值。参数列表中，r表示参考输入值（RC），y表示当前输出值(传感器)，[a] 和 [b]分别表示控制器系数的分子和分母中的系数。另外，上一次函数调用的控制器输出u和传感器输出x也需要参与计算。
void AngleController(float *r, float *y, int n, const float *a, const float *b, float *x, float *u)
{
//PID-Controller
//control error
float e = (*r - *y)*M_PI/180.0;
//calculate plant input
*u = x[n-1]+b[n]*e;
//calculate new controller outputs
for (int i=n-1; i>0; i--)
x[i]=b[i]*e-a[i]*(*u)+x[i-1];
x[0]=b[0]*e-a[0]*(*u);
*u/=4.0;
}这个函数代表yaw的控制器。
void AngleRateController(float *r, float *y, const float *P, float *u)
{
//P-Controller
*u=(*r - *y)*M_PI/(180.0*4) * *P;
}这个函数计算4个电机的PWM信号，基于3个控制器的输出，参考控制器输出。
void CreatePulseWidth(float *u_phi, float *u_theta, float *u_psi_dot, float *u_hover, float *PWM_width)
{
float saturationMax=100;
float saturationMin=10;
if (*u_hover > 5.0){
PWM_width[0]=-*u_theta+*u_phi-*u_psi_dot+*u_hover;
PWM_width[1]=-*u_theta-*u_phi+*u_psi_dot+*u_hover;
PWM_width[2]=*u_theta+*u_phi+*u_psi_dot+*u_hover;
PWM_width[3]=*u_theta-*u_phi-*u_psi_dot+*u_hover;
}
else{
PWM_width[0]=*u_hover;
PWM_width[1]=*u_hover;
PWM_width[2]=*u_hover;
PWM_width[3]=*u_hover;
}
for(i=0;i<4;i++){
if (PWM_width[i]>saturationMax)
PWM_width[i]=saturationMax;
}
}RC接收器放置在四旋翼机臂下，把RC接收到的数据通过串口传给飞控板。串口配置如下：
* 波特率=115200
* 8bits数据位
* 1bit停止位
* 1bit奇偶校验位
接收到的数据帧包含16个字节，格式如下：
其中，1，2，6，9，10字节的数据是常数，5字节的数据在RC断开连接后，会变化。
3，4字节 (THROTTLE) 是整数，代表RC上左摇杆的垂直位置，用来飞机悬停。
左摇杆的水平位置用特定的yaw变化率来控制yaw，对于到的是15，16字节（RUDDER）。
右摇杆的垂直位置 -> roll -> 11,12字节（ELEVATOR）。
右摇杆的水平位置 -> pitch -> 7,8字节（AILERON）。
RC接收器和微控制器之间的数据传输，可以利用DAVE IDE提供的工具来实现。UART001用于串口通信，接收FIFO缓冲器大小设置为32字节，用来一次性读入2帧数据。
第二个工具NVIC002用来产生RC_Receive_ISR的中断信号，这个中断信号会在FIFO缓冲区溢出的时候发出。这在读取到接收数据过程中，是不可少的。（This is required to start the evaluation of the received data.）
关于这部分代码，在RCReceive.c 和 RCReceive.h中。
这个函数把2字节合成一个int，用来把数据帧存到一个变量里面。
int mergeBytes(uint8_t a, uint8_t b) {
int c = a;
return (c << 8) | b;
}这个函数把x从[in_min … in_max]范围映射到[out_min … out_max]。
long map(long x, long in_min, long in_max, long out_min, long out_max){
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}这个函数在FIFO缓冲区溢出时调用。首先检查RC数据帧里面的常数字节，然后从里面抽取需要的数据字节，调用上面两个函数，合并、映射，写入到全局变量中。
void RC_RECEIVE_ISR() {
//Raw values from receiver
int throttleRaw;
int rudderRaw;
int elevatorRaw;
int aileronRaw;
int flightmodeRaw;
uint8_t ReadBufRC[32]; //Readbuffer
int start = 0; //Index of start byte (contains 0x30)
//Check if receive buffer interrupt is set
if(UART001_GetFlagStatus(&RC_UART_Handle,UART001_FIFO_STD_RECV_BUF_FLAG) == UART001_SET){
//Read data from UART buffer
UART001_ReadDataBytes(&RC_UART_Handle,ReadBufRC,32);
//Clear receive buffer interrupt flag
UART001_ClearFlag(&RC_UART_Handle,UART001_FIFO_STD_RECV_BUF_FLAG);
//Search for start byte and check static values
while (ReadBufRC[start] != 0x30 || ReadBufRC[start+1] != 0x00 || ReadBufRC[start+5] != 0xA2 || ReadBufRC[start+8] != 0x2B || ReadBufRC[start+9] != 0xFE)
{
if (start++ > 16) {
//Communication check bytes not in buffer
return;
}
}
//Get data from stream
throttleRaw = mergeBytes(ReadBufRC[start+2],ReadBufRC[start+3]);
throttle = map(throttleRaw, THROTTLE_MIN, THROTTLE_MAX, 0, 60000)/60000.0;
aileronRaw = mergeBytes(ReadBufRC[start+6],ReadBufRC[start+7]);
aileron = map(aileronRaw, AILERON_MIN, AILERON_MAX, -30000, 30000)/30000.0;
elevatorRaw = mergeBytes(ReadBufRC[start+10],ReadBufRC[start+11]);
elevator = map(elevatorRaw, ELEVATOR_MIN, ELEVATOR_MAX, -30000, 30000)/30000.0;
rudderRaw = mergeBytes(ReadBufRC[start+14],ReadBufRC[start+15]);
rudder = map(rudderRaw, RUDDER_MIN, RUDDER_MAX, -30000, 30000)/30000.0;
flightmodeRaw = mergeBytes(ReadBufRC[start+12],ReadBufRC[start+13]);
if (flightmodeRaw == FLIGHTMODE0) flightmode = 0;
if (flightmodeRaw == FLIGHTMODE1) flightmode = 1;
//Set values for RC Timeout check
RCTimeOut = 0;
RCCount++;
}
}这个函数和上面那个很像，但是数据是从蓝牙模块而不是RC。根据flightmode的值，使用Android应用来控制的这个中断函数或者使用RC来控制的上面那个中断函数，会被调用。
void BT_RECEIVE_ISR(){
uint8_t ReadBufRC[20] = {0}; //Readbuffer
int start = 0; //Index of start byte
//Check if receive buffer interrupt is set
if(UART001_GetFlagStatus(&BT_UART_Handle,UART001_FIFO_STD_RECV_BUF_FLAG) == UART001_SET){
//Read data from UART buffer
UART001_ReadDataBytes(&BT_UART_Handle,ReadBufRC,20);
//Clear receive buffer interrupt flag
UART001_ClearFlag(&BT_UART_Handle,UART001_FIFO_STD_RECV_BUF_FLAG);
//Search for start byte and check static values
while (ReadBufRC[start] != 0x02){
if (start++ > 6) {
//Communication check bytes not in buffer
return;
}
}
throttleBT=(ReadBufRC[start+1] << 8 | ReadBufRC[start+2]);
rudderBT=(ReadBufRC[start+3] << 8 | ReadBufRC[start+4]);
elevatorBT=(ReadBufRC[start+5] << 8 | ReadBufRC[start+6]);
aileronBT=(ReadBufRC[start+7] << 8 | ReadBufRC[start+8]);
modeBT=(ReadBufRC[start+9]);
BTTimeOut = 0;
BTCount++;
}
}这个函数是看门狗定时器函数，检查RC数据是否过期了。所以RCCount的值会被周期性地检查。如果自上一次调用之后，这个值一直没有变化，RCTimeOut就置1了。
void WatchRC(void* Temp){
static uint8_t lastCount;
static uint8_t lastBTCount;
if (lastCount == RCCount) RCTimeOut = 1;
lastCount = RCCount;
if (lastBTCount == BTCount) BTTimeOut = 1;
lastBTCount = BTCount;
}这个函数通过创建一个定时器来激活看门狗定时器，每100ms会调用一次WatchRC()。
void WatchRC_Init(){
//Set timer to check every 100ms, if new data has arrived
TimerWatchRC = SYSTM001_CreateTimer(100,SYSTM001_PERIODIC,WatchRC,NULL);
if(TimerWatchRC != 0){
//Timer is created successfully
if(SYSTM001_StartTimer(TimerWatchRC) == DAVEApp_SUCCESS){
//Timer started
}
}
}这个函数在主函数中调用，用来访问RC的值。信号通过SCALE_POWER, SCALE_ROLL, SCALE_PITCH, SCALE_YAW四个常数进行缩放，用来定义遥控杆和指定角度之间的相对关系。
void GetRCData(float* power, float* yaw_dot, float* pitch, float* roll){
if (flightmode == 0){
if (RCTimeOut){
*power=0;
*yaw_dot=0;
*pitch=0;
*roll=0;
}
else{
*power=SCALE_POWER*throttle;
if (rudder > 0.01 || rudder < -0.01)
*yaw_dot=rudder*SCALE_YAW;
else
*yaw_dot = 0;
*pitch=elevator*SCALE_PITCH;
*roll=-aileron*SCALE_ROLL;
}
}
else{
if (BTTimeOut || RCTimeOut){
*power=0;
*yaw_dot=0;
*pitch=0;
*roll=0;
}
else{
*power=SCALE_POWER*(throttleBT/32767.0+1)/2;
*yaw_dot=-rudderBT/32767.0*SCALE_YAW;
if (modeBT == 1){
*pitch=-elevatorBT/32767.0*90;
*roll=aileronBT/32767.0*90;
}
else{
*pitch=elevatorBT/32767.0*SCALE_PITCH;
*roll=aileronBT/32767.0*SCALE_ROLL;
}
}
}
}MPU-9150通过I2C协议传输数据。
它自带一个数字动作处理器，用来处理原始测量数据，并把姿态角，以四元数的形式，存于缓冲区。
为了访问这个单元，需要在初始化芯片的时候写入某些寄存器。Jeff Rowberg 提供了C++代码，用在Arduino。我们用C改写，用在了我们的平台。代码在MPU9150.c 和 MPU9150.h.
DAVE提供的工具EU001, ERU002 and IO002可以用来获取传感器外部中断。当输入上升沿，就会产生中断(NVIC002)，读传感器的缓冲区数据。
为了实现微控制器和传感器之间的通信，可以使用DAVE的工具I2C001。作为要写入MPU-9150中的固件，用来读写的缓冲区要足够大。
I2C通信的代码在I2Cdev.c 和 I2Cdev.h里面.
这个函数用来写传感器的寄存器。参数devAddr包含了MPU-9150的地址，regAddr是打算写入的寄存器的首地址。lenght表示写入的寄存器数量。data包含了要传输的数据。I2C001_WriteData()函数把数据写入到传输缓冲区中，这个函数是由DAVE的工具提供的。当传输成功时，返回TRUE。
写入的过程在一个for循环里面，直到写入成功或者超时（避免无限循环）。
bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data){
I2C001_DataType data1;
data1.Data1.TDF_Type = I2C_TDF_MStart;
data1.Data1.Data = (devAddr | I2C_WRITE);
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return (bool)FALSE;
}
data1.Data1.TDF_Type = I2C_TDF_MTxData;
data1.Data1.Data = regAddr;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return (bool)FALSE;
}
for(int i=0; i<length; i++){
data1.Data1.TDF_Type = I2C_TDF_MTxData;
data1.Data1.Data = data[i];
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return (bool)FALSE;
}
}
data1.Data1.TDF_Type = I2C_TDF_MStop;
data1.Data1.Data = ubyteFF;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return (bool)FALSE;
}
return (bool)TRUE;
}对比I2Cdev_writeBites(…)，这个函数是从传感器中读取数据。在循环中，I2C_TDF_MRxAck0指定从外部寄存器读取数据，I2C_TDF_MRxAck1标识了最后一个寄存器。
int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data){
I2C001_DataType data1;
uint16_t rec=0x00;
int i=0;
data1.Data1.TDF_Type = I2C_TDF_MStart;
data1.Data1.Data = (devAddr | I2C_WRITE);
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return 0;
}
data1.Data1.TDF_Type = I2C_TDF_MTxData;
data1.Data1.Data = regAddr;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return 0;
}
data1.Data1.TDF_Type = I2C_TDF_MRStart;
data1.Data1.Data = devAddr | I2C_READ;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return 0;
}
for (i=0; i<length; i++){
if (i<length-1)
data1.Data1.TDF_Type = I2C_TDF_MRxAck0;
else
data1.Data1.TDF_Type = I2C_TDF_MRxAck1;
data1.Data1.Data = ubyteFF;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return 0;
}
}
data1.Data1.TDF_Type = I2C_TDF_MStop;
data1.Data1.Data = ubyteFF;
for (int cnt=1; !I2C001_WriteData(&I2C001_Handle0, &data1); cnt++){
if (cnt%I2C_TimeOut==0)
return 0;
}
for(i=0; i<length; i++){
for (int cnt=1; !I2C001_ReadData(&I2C001_Handle0, &rec); cnt++)
{
if (cnt%I2C_TimeOut==0)
return 0;
}
data[i]=rec;
}
return i;
}通过DAVE的工具PWMSP001，可以产生4路PWM信号，周期是20ms，脉宽最小5ms，最大10ms。PWM信号是通过定时器实现的。当它到达某个值，就设置输出，或者重置。对于4个PWMSP001这样的实例，这些定时器的分辨率需要重置（the resolution of these timers has to be reset）。由于PWM输出把5V电压下拉到地，需要配置成”open drain”（开漏输出）。
控制器的输出的单位是%，脉宽归一化到5~10ms之间，然后通过PWMSP001_SetDutyCycle函数送到4个PWMSP001实例中。
The program Main.c contains the main function of the controller implementation.
Two arrays PWM_percent and PWM_width contain the current pulsewidth values for motor control both in percent and in ms. x_pitch and x_roll are arrays, which store previous calculations of the controller. The size of these arrays is equal the order of the digital transfer function of the controller.
The float variables u_roll, u_pitch and u_yaw_dot contain the current controller output values.
The controller parameters P, D and N have been determined in the Simulink simulation, the integrator gain has been determined experimentally.
a_roll, a_pitch, b_roll and b_pitch are float arrays that contain the polynomial coefficients of the controller transfer function.
The current quadcopter angles are stored in the three-dimensional array YPR.
After initializing the DAVE-Apps the MPU is initialized, the controller polynomials are determined and the RC-watchdog starts.
Main.c包含了控制器的主程序。
在主循环中，从RC采集数据以及产生PWM信号所需的占空比的值，这两个操作是周期进行的。
DAVE提供的工具USBVC001作为一个图形界面，用来通过串口传递当前的数据到PC上的终端程序中。发送特定编码号到微控制器，就会返回和编码号对应的数据。
初始化之后，MPU-9150以50Hz的频率产生一个外部中断，由ERU_Event_Handler()来评估这个中断。这个函数会检查传感器的数据是不是处理太慢了。如果是的话，传感器的FIFO缓冲区就会提交一个溢出。
基于传感器数据，可以获得空间角度raw、pitch以及yaw变化率。在第一个循环中，定时器TimerController会打开，每过_T_时间调用一次controller。
int main(void){
uint16_t Bytes = 0;
uint16_t nByte;
// status_t status; // Declaration of return variable for DAVE3 APIs (toggle comment if required)
DAVE_Init(); // Initialization of DAVE Apps
USBVC001_Init(); //Initialize the USB core in Device mode
Initialize();
// main loop
while(1){
// Check number of bytes received
Bytes = USBVC001_BytesReceived();
if(Bytes != 0){
for(nByte = 0; nByte < Bytes; nByte++){
// Receive Byte
if(USBVC001_ReceiveByte(&TxBuffer[0]) != DAVEApp_SUCCESS){
//Error
}
}
switch(TxBuffer[0]){
case ‘1‘:
sprintf(c, "Throttle: %f Rudder: %f Elevator: %f Aileron: %f\n", powerD, yawD_dot, pitchD, rollD);
break;
case ‘2‘:
PWMSP001_Start(&PWMSP001_Handle2);
break;
case ‘3‘:
sprintf(c, "Y:%1.2f P:%1.2f R:%1.2f\n", YPR[0], YPR[1], YPR[2]);
break;
case ‘5‘:
sprintf(c, "PWM1:%f PWM2:%f PWM3:%f PWM4:%f\n", PWM_percent[0], PWM_percent[1], PWM_percent[2], PWM_percent[3]);
break;
case ‘6‘:
sprintf(c, "eY:%f eP:%f eR:%f\n", yawD_dot-YPR[0], pitchD-YPR[1], rollD-YPR[2]);
break;
case ‘7‘:
sprintf(c, "TimerSensor:%d TimerMain:%d TimerRC:%d\n", (int)GetSensorCount(), (int)counter_main, (int)GetRCCount());
break;
case ‘8‘:
GetGyroData(sensorData);
sprintf(c, "GyroX:%3.2f GyroY:%3.2f GyroZ:%3.2f\n", sensorData[0], sensorData[1], sensorData[2]);
break;
case ‘9‘:
GetAccData(sensorData);
sprintf(c, "AccX:%f AccY:%f AccZ:%f\n", sensorData[0], sensorData[1], sensorData[2]);
break;
}
USBVC001_SendString((const char *)c);
}
if (sendMag){
sendMag = FALSE;
USBVC001_SendString((const char *)c);
}
// Call continuous to handle class specific control request
USB_USBTask();
}
return 0;
}菊花链协议用来传输数据到PINUS电调板。所以，DaisyChain.c调用了SendDaisyData()这个函数。这个函数会通过串口发送3字节（1个命令字节和2个数据字节）给每个电机，还有一个停止字节，所以总共13个字节。
void SendDaisyData(uint8_t command, uint16_t data1, uint16_t data2, uint16_t data3, uint16_t data4)
{
//Motor 1
DaisyTransmit[0]=command;
DaisyTransmit[1]=(uint8_t)(data1>>8);
DaisyTransmit[2]=(uint8_t)data1;
//Motor 2
DaisyTransmit[3]=command;
DaisyTransmit[4]=(uint8_t)(data2>>8);
DaisyTransmit[5]=(uint8_t) data2;
//Motor 3
DaisyTransmit[6]=command;
DaisyTransmit[7]=(uint8_t)(data3>>8);
DaisyTransmit[8]=(uint8_t) data3;
//Motor 4
DaisyTransmit[9]=command;
DaisyTransmit[10]=(uint8_t)(data4>>8);
DaisyTransmit[11]=(uint8_t) data4;
DaisyTransmit[12]=DAISY_STOP_BYTE;
UART001_WriteDataBytes(&UART001_Handle1, DaisyTransmit, DAISY_BUFFER_SIZE);
}标签：
原文地址：http://blog.csdn.net/duinodu/article/details/51924466
