adc.c /joystick.c /remoter.c 代码详解
（1）adc.c
adc.c 在HARDWARE分组下。
adc.c 主要实现采集摇杆电位器电压 AD 值。
①
//初始化ADC，使用DMA传输
//通道PA0PA1PA3PA4
void Adc_Init（void）
{
GPIO_InitTypeDef GPIO_InitStructure;
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
RCC_APB2PeriphClockCmd（RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB，ENABLE）;//使能GPIOAB时钟
RCC_APB2PeriphClockCmd（RCC_APB2Periph_ADC1，ENABLE）;//使能ADC1时钟
RCC_AHBPeriphClockCmd（RCC_AHBPeriph_DMA1， ENABLE）;//使能DMA时钟
//PA012 作为模拟通道输入引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; //模拟输入引脚
GPIO_Init（GPIOA， &GPIO_InitStructure）;
//PB01 作为模拟通道输入引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
GPIO_Init（GPIOB， &GPIO_InitStructure）;
//DMA 配置
DMA_DeInit（DMA1_Channel1）;
DMA_InitStructure.DMA_PeripheralBaseAddr = （u32）&ADC1-》DR; //ADC1-》DR地址
DMA_InitStructure.DMA_MemoryBaseAddr = （u32）&adc_value;//内存地址
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_BufferSize = 5*ADC_SAMPLE_NUM;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;//外设地址固定
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址增加
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //半字
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; //循环传输
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init（DMA1_Channel1， &DMA_InitStructure）;
DMA_Cmd（DMA1_Channel1， ENABLE）;
ADC_DeInit（ADC1）; //复位ADC1，将外设 ADC1 的全部寄存器重设为缺省值
ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; //ADC工作模式：ADC1和ADC2工作在独立模式
ADC_InitStructure.ADC_ScanConvMode = ENABLE; //扫描模式，用于多通道采集
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; //连续转换模式
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //转换由软件而不是外部触发启动
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //ADC数据右对齐
ADC_InitStructure.ADC_NbrOfChannel = 5; //顺序进行规则转换的ADC通道的数目
ADC_Init（ADC1， &ADC_InitStructure）; //根据ADC_InitStruct中指定的参数初始化外设ADCx的寄存器
ADC_Cmd（ADC1， ENABLE）; //使能指定的ADC1
ADC_DMACmd（ADC1， ENABLE）;//使能ADC1 DMA
RCC_ADCCLKConfig（RCC_PCLK2_Div6）; //设置ADC分频因子6 72M/6=12，ADC最大时间不能超过14M
//配置连续转换通道，55.5个采样周期
ADC_RegularChannelConfig（ADC1， ADC_Channel_0， 1， ADC_SampleTime_239Cycles5）; //1个通道转换一次耗时21us 4个通道
ADC_RegularChannelConfig（ADC1， ADC_Channel_1， 2， ADC_SampleTime_239Cycles5）; //采样个数ADC_SAMPLE_NUM
ADC_RegularChannelConfig（ADC1， ADC_Channel_2， 3， ADC_SampleTime_239Cycles5）; //总共耗时4*21*ADC_SAMPLE_NUM（64）=5.4ms《10ms
ADC_RegularChannelConfig（ADC1， ADC_Channel_8， 4， ADC_SampleTime_239Cycles5）;
ADC_RegularChannelConfig（ADC1， ADC_Channel_9， 5， ADC_SampleTime_239Cycles5）;
ADC_ResetCalibration（ADC1）; //使能复位校准
while（ADC_GetResetCalibrationStatus（ADC1））; //等待复位校准结束
ADC_StartCalibration（ADC1）; //开启AD校准
while（ADC_GetCalibrationStatus（ADC1））; //等待校准结束
ADC_SoftwareStartConvCmd（ADC1， ENABLE）; //使能指定的ADC1的软件转换启动功能
}
DMA（Direct Memory Access）：直接存储器访问。
DMA传输将数据从一个地址空间复制到另一个地址空间。当CPU初始化这个动作后，传输动作本身是由DMA控制器来实现和完成。采用DMA传输数据不仅不会让处理器的工作延迟，反而可以解放处理器去处理其他事项。 DMA传输方式无需CPU直接控制传输，也没有中断处理方式那样保留现场和恢复现场的过程，通过硬件为RAM与I/O设备开辟一条直接传送数据的通路，能使CPU得效率大为提高。 STM32最多有2个DMA控制器，DMA1有7个通道。DMA2有5个通道。每个通道专门用来管理来自于一个或多个外设对存储器访问的请求。还有一个仲裁来协调各个DMA请求的优先权。
②
#define ADC_SAMPLE_NUM 10
u16 adc_value［5*ADC_SAMPLE_NUM］;//ADC采集值存放缓冲区
//ADC均值滤波
void ADC_Filter（u16* adc_val）
{
u16 i=0;
u32 sum［5］={0，0，0，0};
for（;i《ADC_SAMPLE_NUM;i++）
{
sum［0］+=adc_value［5*i+0］;//adc_value是ADC采集出来的数据
sum［1］+=adc_value［5*i+1］;
sum［2］+=adc_value［5*i+2］;
sum［3］+=adc_value［5*i+3］;
sum［4］+=adc_value［5*i+4］;
}
adc_val［0］=sum［0］/ADC_SAMPLE_NUM;
adc_val［1］=sum［1］/ADC_SAMPLE_NUM;
adc_val［2］=sum［2］/ADC_SAMPLE_NUM;
adc_val［3］=sum［3］/ADC_SAMPLE_NUM;
adc_val［4］=sum［4］/ADC_SAMPLE_NUM;
}
③
u16 getAdcValue（u8 axis）
{
u32 sum=0;
for（u8 i=0;i《ADC_SAMPLE_NUM;i++）
{
sum += adc_value［5*i+axis］;
}
return sum/ADC_SAMPLE_NUM;
}
（2）joystick.c
joystick.c在COMMUNICATE分组下。
joystick.c 主要实现将 AD值转为 THRUST、YAW、PITCH、ROLL 对应百分比。
joystick—摇杆，THRUST—油门值
YAW—偏航角，PITCH—俯仰角，ROLL—滚转角
①
/*摇杆初始化*/
void joystickInit（void）
{
if（isInit） return;
Adc_Init（）;
jsParam = &configParam.jsParam;
isInit = true;
}
②
/*获取摇杆ADC值*/
void getFlyDataADCValue（joystickFlyui16_t *adcValue）
{
adcValue-》thrust = getAdcValue（ADC_THRUST）;
adcValue-》roll = getAdcValue（ADC_ROLL）;
adcValue-》pitch = getAdcValue（ADC_PITCH）;
adcValue-》yaw = getAdcValue（ADC_YAW）;
}
③
/*ADC值转换成飞控数据百分比*/
void ADCtoFlyDataPercent（joystickFlyf_t *percent）
{
s16 adcValue;
//THRUST
adcValue = getAdcValue（ADC_THRUST） - jsParam-》thrust.mid;
adcValue = deadband（adcValue，MID_DB_THRUST）;
if（adcValue》=0）
percent-》thrust = （float）adcValue/（jsParam-》thrust.range_pos-MID_DB_THRUST-DB_RANGE）;
else
percent-》thrust = （float）adcValue/（jsParam-》thrust.range_neg-MID_DB_THRUST-DB_RANGE）;
//ROLL
adcValue = getAdcValue（ADC_ROLL） - jsParam-》roll.mid;
adcValue = deadband（adcValue， MID_DB_ROLL）;
if（adcValue 》= 0）
percent-》roll = （float）adcValue/（jsParam-》roll.range_pos-MID_DB_ROLL-DB_RANGE）;
else
percent-》roll = （float）adcValue/（jsParam-》roll.range_neg-MID_DB_ROLL-DB_RANGE）;
//PITCH
adcValue = getAdcValue（ADC_PITCH） - jsParam-》pitch.mid;
adcValue = deadband（adcValue， MID_DB_PITCH）;
if（adcValue 》= 0）
percent-》pitch = （float）adcValue/（jsParam-》pitch.range_pos-MID_DB_PITCH-DB_RANGE）;
else
percent-》pitch = （float）adcValue/（jsParam-》pitch.range_neg-MID_DB_PITCH-DB_RANGE）;
//YAW
adcValue = getAdcValue（ADC_YAW） - jsParam-》yaw.mid;
adcValue = deadband（adcValue， MID_DB_YAW）;
if（adcValue 》= 0）
percent-》yaw = （float）adcValue/（jsParam-》yaw.range_pos-MID_DB_YAW-DB_RANGE）;
else
percent-》yaw = （float）adcValue/（jsParam-》yaw.range_neg-MID_DB_YAW-DB_RANGE）;
}
（3）remoter.c
remoter.c在COMMUNICATE分组下。
remoter.c 主要实现将百分比乘以设定速度值并打包成 ATKP 包格式，然后以 10ms 周期性发送到 radiolink.c 的发送队列中，即 commanderTask。
/*发送飞控命令任务*/
void commanderTask（void* param）
{
float max_thrust = LOW_SPEED_THRUST;
float max_pitch = LOW_SPEED_PITCH;
float max_roll = LOW_SPEED_ROLL;
joystickFlyf_t percent;
while（1）
{
vTaskDelay（10）;
switch（configParam.flight.speed）
{
case LOW_SPEED：
max_thrust = LOW_SPEED_THRUST;
max_pitch = LOW_SPEED_PITCH;
max_roll = LOW_SPEED_ROLL;
break;
case MID_SPEED：
max_thrust = MID_SPEED_THRUST;
max_pitch = MID_SPEED_PITCH;
max_roll = MID_SPEED_ROLL;
break;
case HIGH_SPEED：
max_thrust = HIGH_SPEED_THRUST;
max_pitch = HIGH_SPEED_PITCH;
max_roll = HIGH_SPEED_ROLL;
break;
}
ADCtoFlyDataPercent（&percent）;
//THRUST
if（configParam.flight.ctrl == ALTHOLD_MODE || configParam.flight.ctrl == THREEHOLD_MODE）/*定高模式 和定点模式*/
{
flydata.thrust = percent.thrust * ALT_THRUST;
flydata.thrust += ALT_THRUST;
flydata.thrust = limit（flydata.thrust， 0， 100）;
}
else
{
flydata.thrust = percent.thrust * （max_thrust - MIN_THRUST）;
flydata.thrust += MIN_THRUST;
flydata.thrust = limit（flydata.thrust， MIN_THRUST， max_thrust）;
}
//ROLL
flydata.roll = percent.roll * max_roll;
flydata.roll = limit（flydata.roll， -max_roll， max_roll）;
//PITCH
flydata.pitch = percent.pitch * max_pitch;
flydata.pitch = limit（flydata.pitch， -max_pitch， max_pitch）;
//YAW
flydata.yaw = percent.yaw * MAX_YAW;
flydata.yaw = limit（flydata.yaw， -MAX_YAW， MAX_YAW）;
/*发送飞控数据*/
if（getRCLock（）==false && radioinkConnectStatus（）==true && getIsMFCanFly（）==true）
{
remoterData_t send;
switch（configParam.flight.mode）
{
case HEAD_LESS：
send.flightMode = 1;
break;
case X_MODE：
send.flightMode = 0;
break;
}
switch（configParam.flight.ctrl）
{
case ALTHOLD_MODE：
send.ctrlMode = 1;
break;
case MANUAL_MODE：
send.ctrlMode = 0;
break;
case THREEHOLD_MODE：
send.ctrlMode = 3;
break;
}
if（flydata.thrust《=MIN_THRUST && send.ctrlMode==0）
{
send.thrust = 0;
}
else
{
send.thrust = flydata.thrust;
}
if（getTrimFlag（） == true）
{
send.pitch = 0;
send.roll = 0;
}
else
{
send.pitch = flydata.pitch ;
send.roll = flydata.roll;
}
send.yaw = flydata.yaw;
send.trimPitch = configParam.trim.pitch;
send.trimRoll = configParam.trim.roll;
/*发送飞控数据*/
sendRmotorData（（u8*）&send， sizeof（send））;
}
/*发送遥感数据至匿名上位机*/
if（radioinkConnectStatus（）==true）
{
atkp_t p;
joystickFlyui16_t rcdata;
rcdata.thrust = flydata.thrust*10 + 1000;
rcdata.pitch = percent.pitch*500 + 1500;
rcdata.roll = percent.roll*500 + 1500;
rcdata.yaw = percent.yaw*500 + 1500;
p.msgID = DOWN_RCDATA;
p.dataLen = sizeof（rcdata）;
memcpy（p.data， &rcdata， p.dataLen）;
radiolinkSendPacket（&p）;
}
}
}
adc.c /joystick.c /remoter.c 代码详解
（1）adc.c
adc.c 在HARDWARE分组下。
adc.c 主要实现采集摇杆电位器电压 AD 值。
①
//初始化ADC，使用DMA传输
//通道PA0PA1PA3PA4
void Adc_Init（void）
{
GPIO_InitTypeDef GPIO_InitStructure;
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
RCC_APB2PeriphClockCmd（RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB，ENABLE）;//使能GPIOAB时钟
RCC_APB2PeriphClockCmd（RCC_APB2Periph_ADC1，ENABLE）;//使能ADC1时钟
RCC_AHBPeriphClockCmd（RCC_AHBPeriph_DMA1， ENABLE）;//使能DMA时钟
//PA012 作为模拟通道输入引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; //模拟输入引脚
GPIO_Init（GPIOA， &GPIO_InitStructure）;
//PB01 作为模拟通道输入引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
GPIO_Init（GPIOB， &GPIO_InitStructure）;
//DMA 配置
DMA_DeInit（DMA1_Channel1）;
DMA_InitStructure.DMA_PeripheralBaseAddr = （u32）&ADC1-》DR; //ADC1-》DR地址
DMA_InitStructure.DMA_MemoryBaseAddr = （u32）&adc_value;//内存地址
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_BufferSize = 5*ADC_SAMPLE_NUM;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;//外设地址固定
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //内存地址增加
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; //半字
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; //循环传输
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init（DMA1_Channel1， &DMA_InitStructure）;
DMA_Cmd（DMA1_Channel1， ENABLE）;
ADC_DeInit（ADC1）; //复位ADC1，将外设 ADC1 的全部寄存器重设为缺省值
ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; //ADC工作模式：ADC1和ADC2工作在独立模式
ADC_InitStructure.ADC_ScanConvMode = ENABLE; //扫描模式，用于多通道采集
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; //连续转换模式
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; //转换由软件而不是外部触发启动
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //ADC数据右对齐
ADC_InitStructure.ADC_NbrOfChannel = 5; //顺序进行规则转换的ADC通道的数目
ADC_Init（ADC1， &ADC_InitStructure）; //根据ADC_InitStruct中指定的参数初始化外设ADCx的寄存器
ADC_Cmd（ADC1， ENABLE）; //使能指定的ADC1
ADC_DMACmd（ADC1， ENABLE）;//使能ADC1 DMA
RCC_ADCCLKConfig（RCC_PCLK2_Div6）; //设置ADC分频因子6 72M/6=12，ADC最大时间不能超过14M
//配置连续转换通道，55.5个采样周期
ADC_RegularChannelConfig（ADC1， ADC_Channel_0， 1， ADC_SampleTime_239Cycles5）; //1个通道转换一次耗时21us 4个通道
ADC_RegularChannelConfig（ADC1， ADC_Channel_1， 2， ADC_SampleTime_239Cycles5）; //采样个数ADC_SAMPLE_NUM
ADC_RegularChannelConfig（ADC1， ADC_Channel_2， 3， ADC_SampleTime_239Cycles5）; //总共耗时4*21*ADC_SAMPLE_NUM（64）=5.4ms《10ms
ADC_RegularChannelConfig（ADC1， ADC_Channel_8， 4， ADC_SampleTime_239Cycles5）;
ADC_RegularChannelConfig（ADC1， ADC_Channel_9， 5， ADC_SampleTime_239Cycles5）;
ADC_ResetCalibration（ADC1）; //使能复位校准
while（ADC_GetResetCalibrationStatus（ADC1））; //等待复位校准结束
ADC_StartCalibration（ADC1）; //开启AD校准
while（ADC_GetCalibrationStatus（ADC1））; //等待校准结束
ADC_SoftwareStartConvCmd（ADC1， ENABLE）; //使能指定的ADC1的软件转换启动功能
}
DMA（Direct Memory Access）：直接存储器访问。
DMA传输将数据从一个地址空间复制到另一个地址空间。当CPU初始化这个动作后，传输动作本身是由DMA控制器来实现和完成。采用DMA传输数据不仅不会让处理器的工作延迟，反而可以解放处理器去处理其他事项。 DMA传输方式无需CPU直接控制传输，也没有中断处理方式那样保留现场和恢复现场的过程，通过硬件为RAM与I/O设备开辟一条直接传送数据的通路，能使CPU得效率大为提高。 STM32最多有2个DMA控制器，DMA1有7个通道。DMA2有5个通道。每个通道专门用来管理来自于一个或多个外设对存储器访问的请求。还有一个仲裁来协调各个DMA请求的优先权。
②
#define ADC_SAMPLE_NUM 10
u16 adc_value［5*ADC_SAMPLE_NUM］;//ADC采集值存放缓冲区
//ADC均值滤波
void ADC_Filter（u16* adc_val）
{
u16 i=0;
u32 sum［5］={0，0，0，0};
for（;i《ADC_SAMPLE_NUM;i++）
{
sum［0］+=adc_value［5*i+0］;//adc_value是ADC采集出来的数据
sum［1］+=adc_value［5*i+1］;
sum［2］+=adc_value［5*i+2］;
sum［3］+=adc_value［5*i+3］;
sum［4］+=adc_value［5*i+4］;
}
adc_val［0］=sum［0］/ADC_SAMPLE_NUM;
adc_val［1］=sum［1］/ADC_SAMPLE_NUM;
adc_val［2］=sum［2］/ADC_SAMPLE_NUM;
adc_val［3］=sum［3］/ADC_SAMPLE_NUM;
adc_val［4］=sum［4］/ADC_SAMPLE_NUM;
}
③
u16 getAdcValue（u8 axis）
{
u32 sum=0;
for（u8 i=0;i《ADC_SAMPLE_NUM;i++）
{
sum += adc_value［5*i+axis］;
}
return sum/ADC_SAMPLE_NUM;
}
（2）joystick.c
joystick.c在COMMUNICATE分组下。
joystick.c 主要实现将 AD值转为 THRUST、YAW、PITCH、ROLL 对应百分比。
joystick—摇杆，THRUST—油门值
YAW—偏航角，PITCH—俯仰角，ROLL—滚转角
①
/*摇杆初始化*/
void joystickInit（void）
{
if（isInit） return;
Adc_Init（）;
jsParam = &configParam.jsParam;
isInit = true;
}
②
/*获取摇杆ADC值*/
void getFlyDataADCValue（joystickFlyui16_t *adcValue）
{
adcValue-》thrust = getAdcValue（ADC_THRUST）;
adcValue-》roll = getAdcValue（ADC_ROLL）;
adcValue-》pitch = getAdcValue（ADC_PITCH）;
adcValue-》yaw = getAdcValue（ADC_YAW）;
}
③
/*ADC值转换成飞控数据百分比*/
void ADCtoFlyDataPercent（joystickFlyf_t *percent）
{
s16 adcValue;
//THRUST
adcValue = getAdcValue（ADC_THRUST） - jsParam-》thrust.mid;
adcValue = deadband（adcValue，MID_DB_THRUST）;
if（adcValue》=0）
percent-》thrust = （float）adcValue/（jsParam-》thrust.range_pos-MID_DB_THRUST-DB_RANGE）;
else
percent-》thrust = （float）adcValue/（jsParam-》thrust.range_neg-MID_DB_THRUST-DB_RANGE）;
//ROLL
adcValue = getAdcValue（ADC_ROLL） - jsParam-》roll.mid;
adcValue = deadband（adcValue， MID_DB_ROLL）;
if（adcValue 》= 0）
percent-》roll = （float）adcValue/（jsParam-》roll.range_pos-MID_DB_ROLL-DB_RANGE）;
else
percent-》roll = （float）adcValue/（jsParam-》roll.range_neg-MID_DB_ROLL-DB_RANGE）;
//PITCH
adcValue = getAdcValue（ADC_PITCH） - jsParam-》pitch.mid;
adcValue = deadband（adcValue， MID_DB_PITCH）;
if（adcValue 》= 0）
percent-》pitch = （float）adcValue/（jsParam-》pitch.range_pos-MID_DB_PITCH-DB_RANGE）;
else
percent-》pitch = （float）adcValue/（jsParam-》pitch.range_neg-MID_DB_PITCH-DB_RANGE）;
//YAW
adcValue = getAdcValue（ADC_YAW） - jsParam-》yaw.mid;
adcValue = deadband（adcValue， MID_DB_YAW）;
if（adcValue 》= 0）
percent-》yaw = （float）adcValue/（jsParam-》yaw.range_pos-MID_DB_YAW-DB_RANGE）;
else
percent-》yaw = （float）adcValue/（jsParam-》yaw.range_neg-MID_DB_YAW-DB_RANGE）;
}
（3）remoter.c
remoter.c在COMMUNICATE分组下。
remoter.c 主要实现将百分比乘以设定速度值并打包成 ATKP 包格式，然后以 10ms 周期性发送到 radiolink.c 的发送队列中，即 commanderTask。
/*发送飞控命令任务*/
void commanderTask（void* param）
{
float max_thrust = LOW_SPEED_THRUST;
float max_pitch = LOW_SPEED_PITCH;
float max_roll = LOW_SPEED_ROLL;
joystickFlyf_t percent;
while（1）
{
vTaskDelay（10）;
switch（configParam.flight.speed）
{
case LOW_SPEED：
max_thrust = LOW_SPEED_THRUST;
max_pitch = LOW_SPEED_PITCH;
max_roll = LOW_SPEED_ROLL;
break;
case MID_SPEED：
max_thrust = MID_SPEED_THRUST;
max_pitch = MID_SPEED_PITCH;
max_roll = MID_SPEED_ROLL;
break;
case HIGH_SPEED：
max_thrust = HIGH_SPEED_THRUST;
max_pitch = HIGH_SPEED_PITCH;
max_roll = HIGH_SPEED_ROLL;
break;
}
ADCtoFlyDataPercent（&percent）;
//THRUST
if（configParam.flight.ctrl == ALTHOLD_MODE || configParam.flight.ctrl == THREEHOLD_MODE）/*定高模式 和定点模式*/
{
flydata.thrust = percent.thrust * ALT_THRUST;
flydata.thrust += ALT_THRUST;
flydata.thrust = limit（flydata.thrust， 0， 100）;
}
else
{
flydata.thrust = percent.thrust * （max_thrust - MIN_THRUST）;
flydata.thrust += MIN_THRUST;
flydata.thrust = limit（flydata.thrust， MIN_THRUST， max_thrust）;
}
//ROLL
flydata.roll = percent.roll * max_roll;
flydata.roll = limit（flydata.roll， -max_roll， max_roll）;
//PITCH
flydata.pitch = percent.pitch * max_pitch;
flydata.pitch = limit（flydata.pitch， -max_pitch， max_pitch）;
//YAW
flydata.yaw = percent.yaw * MAX_YAW;
flydata.yaw = limit（flydata.yaw， -MAX_YAW， MAX_YAW）;
/*发送飞控数据*/
if（getRCLock（）==false && radioinkConnectStatus（）==true && getIsMFCanFly（）==true）
{
remoterData_t send;
switch（configParam.flight.mode）
{
case HEAD_LESS：
send.flightMode = 1;
break;
case X_MODE：
send.flightMode = 0;
break;
}
switch（configParam.flight.ctrl）
{
case ALTHOLD_MODE：
send.ctrlMode = 1;
break;
case MANUAL_MODE：
send.ctrlMode = 0;
break;
case THREEHOLD_MODE：
send.ctrlMode = 3;
break;
}
if（flydata.thrust《=MIN_THRUST && send.ctrlMode==0）
{
send.thrust = 0;
}
else
{
send.thrust = flydata.thrust;
}
if（getTrimFlag（） == true）
{
send.pitch = 0;
send.roll = 0;
}
else
{
send.pitch = flydata.pitch ;
send.roll = flydata.roll;
}
send.yaw = flydata.yaw;
send.trimPitch = configParam.trim.pitch;
send.trimRoll = configParam.trim.roll;
/*发送飞控数据*/
sendRmotorData（（u8*）&send， sizeof（send））;
}
/*发送遥感数据至匿名上位机*/
if（radioinkConnectStatus（）==true）
{
atkp_t p;
joystickFlyui16_t rcdata;
rcdata.thrust = flydata.thrust*10 + 1000;
rcdata.pitch = percent.pitch*500 + 1500;
rcdata.roll = percent.roll*500 + 1500;
rcdata.yaw = percent.yaw*500 + 1500;
p.msgID = DOWN_RCDATA;
p.dataLen = sizeof（rcdata）;
memcpy（p.data， &rcdata， p.dataLen）;
radiolinkSendPacket（&p）;
}
}
}
举报
