Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Radio functions in Radio.pde 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@652 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-10-11 13:32:23 +00:00
parent c32ebdd2f5
commit cca79ba0db
4 changed files with 153 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArducopterNG/ArduUser.h
View File

@ -60,10 +60,18 @@ TODO:
/*************************************************************/
// Radio related definitions
// If you don't know these values, you can activate RADIO_TEST_MODE below
// and check your mid values
//#define RADIO_TEST_MODE
#define CH_ROLL 0
#define CH_PITCH 1
#define CH_THROTTLE 2
#define CH_RUDDER 3
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define ROLL_MID 1500 // Radio Roll channel mid value
#define PITCH_MID 1500 // Radio Pitch channel mid value

21
ArducopterNG/Attitude.pde
View File

@ -179,24 +179,3 @@ void Rate_control_v2()
control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
}
// Maximun slope filter for radio inputs... (limit max differences between readings)
int channel_filter(int ch, int ch_old)
{
int diff_ch_old;
if (ch_old==0) // ch_old not initialized
return(ch);
diff_ch_old = ch - ch_old; // Difference with old reading
if (diff_ch_old < 0)
{
if (diff_ch_old <- 60)
return(ch_old - 60); // We limit the max difference between readings
}
else
{
if (diff_ch_old > 60)
return(ch_old + 60);
}
return((ch + ch_old) >> 1); // Small filtering
//return(ch);
}

33
ArducopterNG/Functions.pde
View File

@ -79,3 +79,36 @@ void LightsOff() {
digitalWrite(LED_Red, LOW);
}
// Funtion to normalize an angle in degrees to -180,180 degrees
float Normalize_angle(float angle)
{
if (angle > 180)
return (angle - 360.0);
else if (angle < -180)
return (angle + 360.0);
else
return(angle);
}
// Maximun slope filter for radio inputs... (limit max differences between readings)
int channel_filter(int ch, int ch_old)
{
int diff_ch_old;
if (ch_old==0) // ch_old not initialized
return(ch);
diff_ch_old = ch - ch_old; // Difference with old reading
if (diff_ch_old < 0)
{
if (diff_ch_old <- 60)
return(ch_old - 60); // We limit the max difference between readings
}
else
{
if (diff_ch_old > 60)
return(ch_old + 60);
}
return((ch + ch_old) >> 1); // Small filtering
//return(ch);
}

107
ArducopterNG/Radio.pde
View File

@ -32,3 +32,110 @@ TODO:
* ************************************************************** */
#define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles
#define YAW_STICK_TO_ANGLE_FACTOR 150.0
void Read_radio()
{
if (APM_RC.GetState() == 1) // New radio frame?
{
// Commands from radio Rx
// We apply the Radio calibration parameters (from configurator) except for throttle
ch_roll = channel_filter(APM_RC.InputCh(CH_ROLL) * ch_roll_slope + ch_roll_offset, ch_roll);
ch_pitch = channel_filter(APM_RC.InputCh(CH_PITCH) * ch_pitch_slope + ch_pitch_offset, ch_pitch);
ch_throttle = channel_filter(APM_RC.InputCh(CH_THROTTLE), ch_throttle); // Transmiter calibration not used on throttle
ch_yaw = channel_filter(APM_RC.InputCh(CH_RUDDER) * ch_yaw_slope + ch_yaw_offset, ch_yaw);
ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset;
ch_aux2 = APM_RC.InputCh(CH_6) * ch_aux2_slope + ch_aux2_offset;
// Flight mode
if(ch_aux2 > 1800)
flightMode = 1; // Force to Acro mode from radio
else
flightMode = 0; // Stable mode (default)
// Autopilot mode (only works on Stable mode)
if (flightMode == 0)
{
if(ch_aux > 1800)
AP_mode = 1; // Automatic mode : GPS position hold mode + altitude hold
else
AP_mode = 0; // Normal mode
}
if (flightMode==0) // IN STABLE MODE we convert stick positions to absoulte angles
{
// In Stable mode stick position defines the desired angle in roll, pitch and yaw
#ifdef FLIGHT_MODE_X
// For X mode we make a mix in the input
float aux_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR;
float aux_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
command_rx_roll = aux_roll - aux_pitch;
command_rx_pitch = aux_roll + aux_pitch;
#else
command_rx_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR; // Convert stick position to absolute angles
command_rx_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
#endif
// YAW
if (abs(ch_yaw-yaw_mid)<8) // Take into account a bit of "dead zone" on yaw
aux_float = 0.0;
else
aux_float = (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
command_rx_yaw += aux_float;
command_rx_yaw = Normalize_angle(command_rx_yaw); // Normalize angle to [-180,180]
}
// Write Radio data to DataFlash log
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
} // END new radio data
}
// Send output commands to ESC´s
void Motor_output()
{
// Quadcopter mix
if (motorArmed == 1)
{
#ifdef IsAM
digitalWrite(FR_LED, HIGH); // AM-Mode
#endif
// Quadcopter output mix
rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000);
}
else // MOTORS DISARMED
{
#ifdef IsAM
digitalWrite(FR_LED, LOW); // AM-Mode
#endif
digitalWrite(LED_Green,HIGH); // Ready LED on
rightMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE;
backMotor = MIN_THROTTLE;
// Reset_I_Terms();
roll_I = 0; // reset I terms of PID controls
pitch_I = 0;
yaw_I = 0;
// Initialize yaw command to actual yaw when throttle is down...
command_rx_yaw = ToDeg(yaw);
}
// Send commands to motors
APM_RC.OutputCh(0, rightMotor);
APM_RC.OutputCh(1, leftMotor);
APM_RC.OutputCh(2, frontMotor);
APM_RC.OutputCh(3, backMotor);
// InstantPWM => Force inmediate output on PWM signals
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}