mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 01:48:29 -04:00
ed5c7d01b1
Before: -> After Stabilize P –> Stabilize P (Use NG values, or 8.3 x the older AC2 value) Stabilize I –> Stabilize I (Stays same value) Stabilize D –> Rate P (Stays same value) –> Rate I (new) Added a new value – an I term for rate. The old stabilization routines did not use this term. Please refer to the config.h file to read more about the new PIDs. Added framework for using DCM corrected Accelerometer rates. Code is commented out for now. Added set home at Arming. Crosstrack is now a full PID loop, rather than just a P gain for more control. Throttle now slews when switching out of Alt hold or Auto modes for less jarring transitions Sonar and Baro PIDs are now combined into a throttle PID Yaw control is completely re-written. Added Octa_Quad support - Max git-svn-id: https://arducopter.googlecode.com/svn/trunk@2836 f9c3cf11-9bcb-44bc-f272-b75c42450872
118 lines
2.0 KiB
Plaintext
118 lines
2.0 KiB
Plaintext
void update_lights()
|
|
{
|
|
switch(led_mode){
|
|
case NORMAL_LEDS:
|
|
update_motor_light();
|
|
update_GPS_light();
|
|
break;
|
|
|
|
case AUTO_TRIM_LEDS:
|
|
dancing_light();
|
|
break;
|
|
}
|
|
}
|
|
|
|
void update_GPS_light(void)
|
|
{
|
|
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
|
|
// ---------------------------------------------------------------------
|
|
switch (g_gps->status()){
|
|
|
|
case(2):
|
|
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
|
|
break;
|
|
|
|
case(1):
|
|
if (g_gps->valid_read == true){
|
|
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
|
|
if (GPS_light){
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
}else{
|
|
digitalWrite(C_LED_PIN, HIGH);
|
|
}
|
|
g_gps->valid_read = false;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void update_motor_light(void)
|
|
{
|
|
if(motor_armed == false){
|
|
motor_light = !motor_light;
|
|
|
|
// blink
|
|
if(motor_light){
|
|
digitalWrite(A_LED_PIN, HIGH);
|
|
}else{
|
|
digitalWrite(A_LED_PIN, LOW);
|
|
}
|
|
}else{
|
|
if(!motor_light){
|
|
motor_light = true;
|
|
digitalWrite(A_LED_PIN, HIGH);
|
|
}
|
|
}
|
|
}
|
|
|
|
void dancing_light()
|
|
{
|
|
static byte step;
|
|
|
|
if (step++ == 3)
|
|
step = 0;
|
|
|
|
switch(step)
|
|
{
|
|
case 0:
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
digitalWrite(A_LED_PIN, HIGH);
|
|
break;
|
|
|
|
case 1:
|
|
digitalWrite(A_LED_PIN, LOW);
|
|
digitalWrite(B_LED_PIN, HIGH);
|
|
break;
|
|
|
|
case 2:
|
|
digitalWrite(B_LED_PIN, LOW);
|
|
digitalWrite(C_LED_PIN, HIGH);
|
|
break;
|
|
}
|
|
}
|
|
void clear_leds()
|
|
{
|
|
digitalWrite(A_LED_PIN, LOW);
|
|
digitalWrite(B_LED_PIN, LOW);
|
|
digitalWrite(C_LED_PIN, LOW);
|
|
}
|
|
|
|
#if MOTOR_LEDS == 1
|
|
void update_motor_leds(void)
|
|
{
|
|
// blink rear
|
|
static bool blink;
|
|
|
|
if (blink){
|
|
blink = false;
|
|
digitalWrite(RE_LED, HIGH);
|
|
digitalWrite(FR_LED, HIGH);
|
|
digitalWrite(RI_LED, LOW);
|
|
digitalWrite(LE_LED, LOW);
|
|
|
|
}else{
|
|
blink = true;
|
|
digitalWrite(RE_LED, LOW);
|
|
digitalWrite(FR_LED, LOW);
|
|
digitalWrite(RI_LED, HIGH);
|
|
digitalWrite(LE_LED, HIGH);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|