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

Copter: integrate notify

This commit is contained in:
Randy Mackay 2013-08-08 22:15:25 +09:00 committed by Andrew Tridgell
parent f8a872f01e
commit e985253f1a
9 changed files with 16 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/ArduCopter.pde
View File

@ -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;
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
@ -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;
////////////////////////////////////////////////////////////////////////////////
// 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() {
// 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()
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)
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
View File

@ -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 @@
# 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 @@
# 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 @@
# 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 @@
# 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
View File

@ -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()
// 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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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)
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)
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)
*/
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
View File

@ -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) {