Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduCopter/radio.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 17:50:02 -07:00 committed by Pat Hickey
parent bcad02a1ba
commit c9953574ca
1 changed files with 158 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

316
ArduCopter/radio.pde
View File

@ -2,214 +2,214 @@
//Function that will read the radio data, limit servos and trigger a failsafe //Function that will read the radio data, limit servos and trigger a failsafe
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
extern RC_Channel* rc_ch[NUM_CHANNELS]; extern RC_Channel* rc_ch[NUM_CHANNELS];
static void default_dead_zones() static void default_dead_zones()
{ {
g.rc_1.set_dead_zone(60); g.rc_1.set_dead_zone(60);
g.rc_2.set_dead_zone(60); g.rc_2.set_dead_zone(60);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.rc_3.set_dead_zone(20); g.rc_3.set_dead_zone(20);
g.rc_4.set_dead_zone(30); g.rc_4.set_dead_zone(30);
#else #else
g.rc_3.set_dead_zone(60); g.rc_3.set_dead_zone(60);
g.rc_4.set_dead_zone(80); g.rc_4.set_dead_zone(80);
#endif #endif
} }
static void init_rc_in() static void init_rc_in()
{ {
// set rc channel ranges // set rc channel ranges
g.rc_1.set_angle(4500); g.rc_1.set_angle(4500);
g.rc_2.set_angle(4500); g.rc_2.set_angle(4500);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// we do not want to limit the movment of the heli's swash plate // we do not want to limit the movment of the heli's swash plate
g.rc_3.set_range(0, 1000); g.rc_3.set_range(0, 1000);
#else #else
g.rc_3.set_range(g.throttle_min, g.throttle_max); g.rc_3.set_range(g.throttle_min, g.throttle_max);
#endif #endif
g.rc_4.set_angle(4500); g.rc_4.set_angle(4500);
// reverse: CW = left // reverse: CW = left
// normal: CW = left??? // normal: CW = left???
g.rc_1.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_1.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW);
g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
rc_ch[CH_1] = &g.rc_1; rc_ch[CH_1] = &g.rc_1;
rc_ch[CH_2] = &g.rc_2; rc_ch[CH_2] = &g.rc_2;
rc_ch[CH_3] = &g.rc_3; rc_ch[CH_3] = &g.rc_3;
rc_ch[CH_4] = &g.rc_4; rc_ch[CH_4] = &g.rc_4;
rc_ch[CH_5] = &g.rc_5; rc_ch[CH_5] = &g.rc_5;
rc_ch[CH_6] = &g.rc_6; rc_ch[CH_6] = &g.rc_6;
rc_ch[CH_7] = &g.rc_7; rc_ch[CH_7] = &g.rc_7;
rc_ch[CH_8] = &g.rc_8; rc_ch[CH_8] = &g.rc_8;
//set auxiliary ranges //set auxiliary ranges
g.rc_5.set_range(0,1000); g.rc_5.set_range(0,1000);
g.rc_6.set_range(0,1000); g.rc_6.set_range(0,1000);
g.rc_7.set_range(0,1000); g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);
#if MOUNT == ENABLED #if MOUNT == ENABLED
update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11); update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11);
#endif #endif
} }
static void init_rc_out() static void init_rc_out()
{ {
APM_RC.Init( &isr_registry ); // APM Radio initialization APM_RC.Init( &isr_registry ); // APM Radio initialization
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
motors.set_update_rate(AP_MOTORS_SPEED_INSTANT_PWM); motors.set_update_rate(AP_MOTORS_SPEED_INSTANT_PWM);
#else #else
motors.set_update_rate(g.rc_speed); motors.set_update_rate(g.rc_speed);
#endif #endif
motors.set_frame_orientation(g.frame_orientation); motors.set_frame_orientation(g.frame_orientation);
motors.Init(); // motor initialisation motors.Init(); // motor initialisation
motors.set_min_throttle(g.throttle_min); motors.set_min_throttle(g.throttle_min);
motors.set_max_throttle(g.throttle_max); motors.set_max_throttle(g.throttle_max);
for(byte i = 0; i < 5; i++){ for(byte i = 0; i < 5; i++) {
delay(20); delay(20);
read_radio(); read_radio();
} }
// we want the input to be scaled correctly // we want the input to be scaled correctly
g.rc_3.set_range_out(0,1000); g.rc_3.set_range_out(0,1000);
// sanity check - prevent unconfigured radios from outputting // sanity check - prevent unconfigured radios from outputting
if(g.rc_3.radio_min >= 1300){ if(g.rc_3.radio_min >= 1300) {
g.rc_3.radio_min = g.rc_3.radio_in; g.rc_3.radio_min = g.rc_3.radio_in;
} }
// we are full throttle // we are full throttle
if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)){ if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)) {
if(g.esc_calibrate == 0){ if(g.esc_calibrate == 0) {
// we will enter esc_calibrate mode on next reboot // we will enter esc_calibrate mode on next reboot
g.esc_calibrate.set_and_save(1); g.esc_calibrate.set_and_save(1);
// send miinimum throttle out to ESC // send miinimum throttle out to ESC
motors.output_min(); motors.output_min();
// block until we restart // block until we restart
while(1){ while(1) {
//Serial.println("esc"); //Serial.println("esc");
delay(200); delay(200);
dancing_light(); dancing_light();
} }
}else{ }else{
//Serial.println("esc init"); //Serial.println("esc init");
// clear esc flag // clear esc flag
g.esc_calibrate.set_and_save(0); g.esc_calibrate.set_and_save(0);
// block until we restart // block until we restart
init_esc(); init_esc();
} }
}else{ }else{
// did we abort the calibration? // did we abort the calibration?
if(g.esc_calibrate == 1) if(g.esc_calibrate == 1)
g.esc_calibrate.set_and_save(0); g.esc_calibrate.set_and_save(0);
// send miinimum throttle out to ESC // send miinimum throttle out to ESC
output_min(); output_min();
} }
#if TOY_EDF == ENABLED #if TOY_EDF == ENABLED
// add access to CH8 and CH6 // add access to CH8 and CH6
APM_RC.enable_out(CH_8); APM_RC.enable_out(CH_8);
APM_RC.enable_out(CH_6); APM_RC.enable_out(CH_6);
#endif #endif
} }
void output_min() void output_min()
{ {
// enable motors // enable motors
motors.enable(); motors.enable();
motors.output_min(); motors.output_min();
} }
static void read_radio() static void read_radio()
{ {
if (APM_RC.GetState() == 1){ if (APM_RC.GetState() == 1) {
new_radio_frame = true; new_radio_frame = true;
g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
g.rc_4.set_pwm(APM_RC.InputCh(CH_4)); g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
#if FRAME_CONFIG != HELI_FRAME #if FRAME_CONFIG != HELI_FRAME
// limit our input to 800 so we can still pitch and roll // limit our input to 800 so we can still pitch and roll
g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE); g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE);
#endif #endif
throttle_failsafe(g.rc_3.radio_in); throttle_failsafe(g.rc_3.radio_in);
} }
} }
#define FS_COUNTER 3 #define FS_COUNTER 3
static void throttle_failsafe(uint16_t pwm) static void throttle_failsafe(uint16_t pwm)
{ {
// Don't enter Failsafe if not enabled by user // Don't enter Failsafe if not enabled by user
if(g.throttle_fs_enabled == 0) if(g.throttle_fs_enabled == 0)
return; return;
//check for failsafe and debounce funky reads //check for failsafe and debounce funky reads
// ------------------------------------------ // ------------------------------------------
if (pwm < (unsigned)g.throttle_fs_value){ if (pwm < (unsigned)g.throttle_fs_value) {
// we detect a failsafe from radio // we detect a failsafe from radio
// throttle has dropped below the mark // throttle has dropped below the mark
failsafeCounter++; failsafeCounter++;
if (failsafeCounter == FS_COUNTER-1){ if (failsafeCounter == FS_COUNTER-1) {
// called right before trigger // called right before trigger
// do nothing // do nothing
}else if(failsafeCounter == FS_COUNTER) { }else if(failsafeCounter == FS_COUNTER) {
// Don't enter Failsafe if we are not armed // Don't enter Failsafe if we are not armed
// home distance is in meters // home distance is in meters
// This is to prevent accidental RTL // This is to prevent accidental RTL
if(motors.armed() && takeoff_complete){ if(motors.armed() && takeoff_complete) {
SendDebug("MSG FS ON "); SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC); SendDebugln(pwm, DEC);
set_failsafe(true); set_failsafe(true);
} }
}else if (failsafeCounter > FS_COUNTER){ }else if (failsafeCounter > FS_COUNTER) {
failsafeCounter = FS_COUNTER+1; failsafeCounter = FS_COUNTER+1;
} }
}else if(failsafeCounter > 0){ }else if(failsafeCounter > 0) {
// we are no longer in failsafe condition // we are no longer in failsafe condition
// but we need to recover quickly // but we need to recover quickly
failsafeCounter--; failsafeCounter--;
if (failsafeCounter > 3){ if (failsafeCounter > 3) {
failsafeCounter = 3; failsafeCounter = 3;
} }
if (failsafeCounter == 1){ if (failsafeCounter == 1) {
SendDebug("MSG FS OFF "); SendDebug("MSG FS OFF ");
SendDebugln(pwm, DEC); SendDebugln(pwm, DEC);
}else if(failsafeCounter == 0) { }else if(failsafeCounter == 0) {
set_failsafe(false); set_failsafe(false);
}else if (failsafeCounter <0){ }else if (failsafeCounter <0) {
failsafeCounter = -1; failsafeCounter = -1;
} }
} }
} }
static void trim_radio() static void trim_radio()
{ {
for (byte i = 0; i < 30; i++){ for (byte i = 0; i < 30; i++) {
read_radio(); read_radio();
} }
g.rc_1.trim(); // roll g.rc_1.trim(); // roll
g.rc_2.trim(); // pitch g.rc_2.trim(); // pitch
g.rc_4.trim(); // yaw g.rc_4.trim(); // yaw
g.rc_1.save_eeprom(); g.rc_1.save_eeprom();
g.rc_2.save_eeprom(); g.rc_2.save_eeprom();
g.rc_4.save_eeprom(); g.rc_4.save_eeprom();
} }