zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: integrate notify

mission-4.1.18
Randy Mackay 12 years ago committed by Andrew Tridgell
parent
f8a872f01e
commit
e985253f1a
9 changed files with 16 additions and 214 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      ArduCopter/ArduCopter.pde
  2. 15
      ArduCopter/config.h
  3. 5
      ArduCopter/control_modes.pde
  4. 157
      ArduCopter/leds.pde
  5. 3
      ArduCopter/motors.pde
  6. 7
      ArduCopter/radio.pde
  7. 2
      ArduCopter/setup.pde
  8. 22
      ArduCopter/system.pde
  9. 4
      ArduCopter/test.pde

15
ArduCopter/ArduCopter.pde
Unescape View File

@ -73,6 +73,8 @@ @@ -73,6 +73,8 @@
#include <AP_HAL_Empty.h>
// Application dependencies
#include <AP_Notify.h> // Notify library
#include <AP_BoardLED.h> // BoardLEDs library
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_GPS.h> // ArduPilot GPS library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
@ -133,7 +135,6 @@ static AP_HAL::BetterStream* cliSerial; @@ -133,7 +135,6 @@ static AP_HAL::BetterStream* cliSerial;
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
@ -145,6 +146,9 @@ static Parameters g; @@ -145,6 +146,9 @@ static Parameters g;
// main loop scheduler
static AP_Scheduler scheduler;
// AP_BoardLED instance
static AP_BoardLED board_led;
////////////////////////////////////////////////////////////////////////////////
// prototypes
////////////////////////////////////////////////////////////////////////////////
@ -461,9 +465,6 @@ static uint8_t pid_log_counter; @@ -461,9 +465,6 @@ static uint8_t pid_log_counter;
////////////////////////////////////////////////////////////////////////////////
// LED output
////////////////////////////////////////////////////////////////////////////////
// This is current status for the LED lights state machine
// setting this value changes the output of the LEDs
static uint8_t led_mode = NORMAL_LEDS;
// Blinking indicates GPS status
static uint8_t copter_leds_GPS_blink;
// Blinking indicates battery status
@ -884,6 +885,9 @@ void setup() { @@ -884,6 +885,9 @@ void setup() {
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
// initialise board leds
board_led.init();
#if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar_analog_source = new AP_ADC_AnalogSource(
@ -1190,8 +1194,6 @@ static void medium_loop() @@ -1190,8 +1194,6 @@ static void medium_loop()
USERHOOK_MEDIUMLOOP
#endif
// update board leds
update_board_leds();
#if COPTER_LEDS == ENABLED
update_copter_leds();
#endif
@ -1355,7 +1357,6 @@ static void update_GPS(void) @@ -1355,7 +1357,6 @@ static void update_GPS(void)
static uint8_t ground_start_count = 10;
g_gps->update();
update_GPS_light();
if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
// clear new data flag

15
ArduCopter/config.h
Unescape View File

@ -168,9 +168,6 @@ @@ -168,9 +168,6 @@
// LED and IO Pins
//
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define A_LED_PIN 37
# define B_LED_PIN 36
# define C_LED_PIN 35
# define LED_ON HIGH
# define LED_OFF LOW
# define PUSHBUTTON_PIN 41
@ -178,9 +175,6 @@ @@ -178,9 +175,6 @@
# define BATTERY_VOLT_PIN 0 // Battery voltage on A0
# define BATTERY_CURR_PIN 1 // Battery current on A1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define PUSHBUTTON_PIN (-1)
@ -188,9 +182,6 @@ @@ -188,9 +182,6 @@
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define PUSHBUTTON_PIN (-1)
@ -198,9 +189,6 @@ @@ -198,9 +189,6 @@
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define PUSHBUTTON_PIN (-1)
@ -209,9 +197,6 @@ @@ -209,9 +197,6 @@
# define BATTERY_CURR_PIN -1
#elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
// XXX these are just copied, may not make sense
# define A_LED_PIN 27
# define B_LED_PIN 26
# define C_LED_PIN 25
# define LED_ON LOW
# define LED_OFF HIGH
# define PUSHBUTTON_PIN (-1)

5
ArduCopter/control_modes.pde
Unescape View File

@ -295,7 +295,7 @@ static void auto_trim() @@ -295,7 +295,7 @@ static void auto_trim()
auto_trim_counter--;
// flash the leds
led_mode = SAVE_TRIM_LEDS;
notify.flags.save_trim = true;
// calculate roll trim adjustment
float roll_trim_adjustment = ToRad((float)g.rc_1.control_in / 4000.0f);
@ -313,8 +313,7 @@ static void auto_trim() @@ -313,8 +313,7 @@ static void auto_trim()
// on last iteration restore leds and accel gains to normal
if(auto_trim_counter == 0) {
ahrs.set_fast_gains(false);
led_mode = NORMAL_LEDS;
clear_leds();
notify.flags.save_trim = false;
}
}
}

157
ArduCopter/leds.pde
Unescape View File

@ -1,162 +1,5 @@ @@ -1,162 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// update_board_leds - updates leds on board
// should be called at 10hz
static void update_board_leds()
{
// we need to slow down the calls to the GPS and dancing lights to 3.33hz
static uint8_t counter = 0;
if(++counter >= 3){
counter = 0;
}
switch(led_mode) {
case NORMAL_LEDS:
update_arming_light();
if (counter == 0) {
update_GPS_light();
}
break;
case SAVE_TRIM_LEDS:
if (counter == 0) {
dancing_light();
}
break;
}
}
static 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 GPS::NO_FIX:
case GPS::GPS_OK_FIX_2D:
// check if we've blinked since the last gps update
if (g_gps->valid_read) {
g_gps->valid_read = false;
ap_system.GPS_light = !ap_system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (ap_system.GPS_light) {
digitalWrite(C_LED_PIN, LED_OFF);
}else{
digitalWrite(C_LED_PIN, LED_ON);
}
}
break;
case GPS::GPS_OK_FIX_3D:
if(ap.home_is_set) {
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
} else {
digitalWrite(C_LED_PIN, LED_OFF);
}
break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
break;
}
}
static void update_arming_light(void)
{
// counter to control state
static int8_t counter = 0;
counter++;
// disarmed
if(!motors.armed()) {
if(!ap.pre_arm_check) {
// failed pre-arm checks so double flash
switch(counter) {
case 0:
ap_system.arming_light = true;
break;
case 1:
ap_system.arming_light = false;
break;
case 2:
ap_system.arming_light = true;
break;
case 3:
case 4:
ap_system.arming_light = false;
break;
default:
// reset counter to restart the sequence
counter = -1;
break;
}
}else{
// passed pre-arm checks so slower single flash
switch(counter) {
case 0:
case 1:
case 2:
ap_system.arming_light = true;
break;
case 3:
case 4:
case 5:
ap_system.arming_light = false;
break;
default:
// reset counter to restart the sequence
counter = -1;
break;
}
}
// set arming led from arming_light flag
if(ap_system.arming_light) {
digitalWrite(A_LED_PIN, LED_ON);
}else{
digitalWrite(A_LED_PIN, LED_OFF);
}
}else{
// armed
if(!ap_system.arming_light) {
ap_system.arming_light = true;
digitalWrite(A_LED_PIN, LED_ON);
}
}
}
static void dancing_light()
{
static uint8_t step;
if (step++ == 3)
step = 0;
switch(step)
{
case 0:
digitalWrite(C_LED_PIN, LED_OFF);
digitalWrite(A_LED_PIN, LED_ON);
break;
case 1:
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_ON);
break;
case 2:
digitalWrite(B_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_ON);
break;
}
}
static void clear_leds()
{
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF);
ap_system.arming_light = false;
led_mode = NORMAL_LEDS;
}
/////////////////////////////////////////////////////////////////////////////////////////////
// Copter LEDS by Robert Lefebvre
// Based on the work of U4eake, Bill Sanford, Max Levine, and Oliver

3
ArduCopter/motors.pde
Unescape View File

@ -182,9 +182,6 @@ static void init_arm_motors() @@ -182,9 +182,6 @@ static void init_arm_motors()
// set hover throttle
motors.set_mid_throttle(g.throttle_mid);
// update leds on board
update_arming_light();
#if COPTER_LEDS == ENABLED
piezo_beep_twice();
#endif

7
ArduCopter/radio.pde
Unescape View File

@ -74,11 +74,10 @@ static void init_rc_out() @@ -74,11 +74,10 @@ static void init_rc_out()
g.esc_calibrate.set_and_save(1);
// display message on console
cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n"));
// turn on esc calibration notification
notify.flags.esc_calibration = true;
// block until we restart
while(1) {
delay(200);
dancing_light();
}
while(1) {}
}else{
cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n"));
// clear esc flag

2
ArduCopter/setup.pde
Unescape View File

@ -1356,7 +1356,7 @@ init_esc() @@ -1356,7 +1356,7 @@ init_esc()
while(1) {
read_radio();
delay(100);
dancing_light();
notify.flags.esc_calibration = true;
motors.throttle_pass_through();
}
}

22
ArduCopter/system.pde
Unescape View File

@ -123,16 +123,6 @@ static void init_ardupilot() @@ -123,16 +123,6 @@ static void init_ardupilot()
//
report_version();
// setup IO pins
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
digitalWrite(A_LED_PIN, LED_OFF);
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
digitalWrite(B_LED_PIN, LED_OFF);
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
digitalWrite(C_LED_PIN, LED_OFF);
relay.init();
#if COPTER_LEDS == ENABLED
@ -303,10 +293,6 @@ static void startup_ground(void) @@ -303,10 +293,6 @@ static void startup_ground(void)
ahrs2.set_fast_gains(true);
#endif
// reset the leds
// ---------------------------
clear_leds();
// when we re-calibrate the gyros,
// all previous I values are invalid
reset_I_all();
@ -367,10 +353,6 @@ static bool set_mode(uint8_t mode) @@ -367,10 +353,6 @@ static bool set_mode(uint8_t mode)
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// report the GPS and Motor arming status
// To-Do: this should be initialised somewhere else related to the LEDs
led_mode = NORMAL_LEDS;
switch(mode) {
case ACRO:
success = true;
@ -632,8 +614,8 @@ static void check_usb_mux(void) @@ -632,8 +614,8 @@ static void check_usb_mux(void)
*/
void flash_leds(bool on)
{
digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
//digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
//digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
}
/*

4
ArduCopter/test.pde
Unescape View File

@ -264,10 +264,6 @@ test_gps(uint8_t argc, const Menu::arg *argv) @@ -264,10 +264,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
while(1) {
delay(100);
// Blink GPS LED if we don't have a fix
// ------------------------------------
update_GPS_light();
g_gps->update();
if (g_gps->new_data) {

Write Preview
Loading…
Cancel
Save