zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ArduCopter: replaced digitalRead and digitalWrite with faster calls 

improved performance logging to dataflash
master
rmackay9 12 years ago
parent
67fadd337d
commit
07a7a1acd8
10 changed files with 170 additions and 116 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. 41
      ArduCopter/ArduCopter.pde
  2. 8
      ArduCopter/Attitude.pde
  3. 23
      ArduCopter/Log.pde
  4. 6
      ArduCopter/config.h
  5. 136
      ArduCopter/leds.pde
  6. 50
      ArduCopter/perf_info.pde
  7. 4
      ArduCopter/sensors.pde
  8. 8
      ArduCopter/setup.pde
  9. 6
      ArduCopter/system.pde
  10. 4
      ArduCopter/test.pde

41
ArduCopter/ArduCopter.pde
Unescape View File

@ -108,6 +108,7 @@ @@ -108,6 +108,7 @@
#include <AP_Airspeed.h> // needed for AHRS build
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <ThirdOrderCompFilter.h> // Complementary filter for combining barometer altitude with accelerometers
#include <DigitalWriteFast.h> // faster digital write for LEDs
#include <memcheck.h>
// Configuration
@ -230,7 +231,7 @@ AP_Baro_MS5611 barometer; @@ -230,7 +231,7 @@ AP_Baro_MS5611 barometer;
AP_Compass_HMC5843 compass;
#endif
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN);
#else
@ -307,17 +308,18 @@ AP_GPS_HIL g_gps_driver(NULL); @@ -307,17 +308,18 @@ AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_Baro_BMP085_HIL barometer;
#ifdef OPTFLOW_ENABLED
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#if OPTFLOW == ENABLED
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN);
#else
#else
AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN);
#endif
#endif
#endif // CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
#endif // OPTFLOW == ENABLED
#ifdef DESKTOP_BUILD
#include <SITL.h>
SITL sitl;
#endif
#endif // DESKTOP_BUILD
static int32_t gps_base_alt;
#else
#error Unrecognised HIL_MODE setting.
@ -942,7 +944,9 @@ void loop() @@ -942,7 +944,9 @@ void loop()
Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer));
#endif
//PORTK |= B00010000;
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
@ -965,9 +969,6 @@ void loop() @@ -965,9 +969,6 @@ void loop()
// store the micros for the 50 hz timer
fiftyhz_loopTimer = timer;
// port manipulation for external timing of main loops
//PORTK |= B01000000;
// check for new GPS messages
// --------------------------
update_GPS();
@ -991,14 +992,13 @@ void loop() @@ -991,14 +992,13 @@ void loop()
counter_one_herz = 0;
}
perf_mon_counter++;
if (perf_mon_counter > 600 ) {
if (perf_mon_counter >= 500 ) { // 500 iterations at 50hz = 10 seconds
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
perf_info_reset();
gps_fix_count = 0;
perf_mon_counter = 0;
}
//PORTK &= B10111111;
}
} else {
#ifdef DESKTOP_BUILD
@ -1017,12 +1017,7 @@ void loop() @@ -1017,12 +1017,7 @@ void loop()
}
}
// port manipulation for external timing of main loops
//PORTK &= B11101111;
}
// PORTK |= B01000000;
// PORTK &= B10111111;
// Main loop - 100hz
static void fast_loop()
@ -1052,11 +1047,11 @@ static void fast_loop() @@ -1052,11 +1047,11 @@ static void fast_loop()
// optical flow
// --------------------
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
if(g.optflow_enabled) {
update_optical_flow();
}
#endif
#endif // OPTFLOW == ENABLED
// Read radio and 3-position switch on radio
// -----------------------------------------
@ -1397,7 +1392,7 @@ static void super_slow_loop() @@ -1397,7 +1392,7 @@ static void super_slow_loop()
}
// called at 100hz but data from sensor only arrives at 20 Hz
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
static void update_optical_flow(void)
{
static uint32_t last_of_update = 0;
@ -1418,7 +1413,7 @@ static void update_optical_flow(void) @@ -1418,7 +1413,7 @@ static void update_optical_flow(void)
}
}
}
#endif
#endif // OPTFLOW == ENABLED
// called at 50hz
static void update_GPS(void)

8
ArduCopter/Attitude.pde
Unescape View File

@ -600,7 +600,7 @@ static int16_t heli_get_angle_boost(int16_t throttle) @@ -600,7 +600,7 @@ static int16_t heli_get_angle_boost(int16_t throttle)
static int32_t
get_of_roll(int32_t input_roll)
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
static float tot_x_cm = 0; // total distance from target
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
@ -648,13 +648,13 @@ get_of_roll(int32_t input_roll) @@ -648,13 +648,13 @@ get_of_roll(int32_t input_roll)
return input_roll+of_roll;
#else
return input_roll;
#endif
#endif // OPTFLOW == ENABLED
}
static int32_t
get_of_pitch(int32_t input_pitch)
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
static float tot_y_cm = 0; // total distance from target
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
@ -703,5 +703,5 @@ get_of_pitch(int32_t input_pitch) @@ -703,5 +703,5 @@ get_of_pitch(int32_t input_pitch)
return input_pitch+of_pitch;
#else
return input_pitch;
#endif
#endif // OPTFLOW == ENABLED
}

23
ArduCopter/Log.pde
Unescape View File

@ -508,7 +508,7 @@ static void Log_Read_Motors() @@ -508,7 +508,7 @@ static void Log_Read_Motors()
// Write an optical flow packet. Total length : 30 bytes
static void Log_Write_Optflow()
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_OPTFLOW_MSG);
@ -522,13 +522,12 @@ static void Log_Write_Optflow() @@ -522,13 +522,12 @@ static void Log_Write_Optflow()
DataFlash.WriteLong(of_roll);
DataFlash.WriteLong(of_pitch);
DataFlash.WriteByte(END_BYTE);
#endif
#endif // OPTFLOW == ENABLED
}
// Read an optical flow packet.
static void Log_Read_Optflow()
{
#ifdef OPTFLOW_ENABLED
int16_t temp1 = DataFlash.ReadInt(); // 1
int16_t temp2 = DataFlash.ReadInt(); // 2
int16_t temp3 = DataFlash.ReadInt(); // 3
@ -549,7 +548,6 @@ static void Log_Read_Optflow() @@ -549,7 +548,6 @@ static void Log_Read_Optflow()
temp7,
(long)temp8,
(long)temp9);
#endif
}
// Write an Nav Tuning packet. Total length : 24 bytes
@ -673,10 +671,13 @@ static void Log_Write_Performance() @@ -673,10 +671,13 @@ static void Log_Write_Performance()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteByte(0); //1 - was adc_constraints
DataFlash.WriteByte(0); //1
DataFlash.WriteByte(ahrs.renorm_range_count); //2
DataFlash.WriteByte(ahrs.renorm_blowup_count); //3
DataFlash.WriteByte(gps_fix_count); //4
DataFlash.WriteInt(perf_info_get_num_long_running()); //5 - number of long running loops
DataFlash.WriteLong(perf_info_get_num_loops()); //6 - total number of loops
DataFlash.WriteLong(perf_info_get_max_time()); //7 - time of longest running loop
DataFlash.WriteByte(END_BYTE);
}
@ -687,13 +688,19 @@ static void Log_Read_Performance() @@ -687,13 +688,19 @@ static void Log_Read_Performance()
int8_t temp2 = DataFlash.ReadByte();
int8_t temp3 = DataFlash.ReadByte();
int8_t temp4 = DataFlash.ReadByte();
uint16_t temp5 = DataFlash.ReadInt();
uint32_t temp6 = DataFlash.ReadLong();
uint32_t temp7 = DataFlash.ReadLong();
//1 2 3 4
Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"),
// 1 2 3 4 5 6 7
Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %u, %lu, %lu\n"),
(int)temp1,
(int)temp2,
(int)temp3,
(int)temp4);
(int)temp4,
temp5,
temp6,
temp7);
}
// Write a command processing packet. Total length : 21 bytes

6
ArduCopter/config.h
Unescape View File

@ -348,10 +348,10 @@ @@ -348,10 +348,10 @@
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included
#define OPTFLOW_ENABLED
#define OPTFLOW ENABLED
#endif
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
# define OPTFLOW DISABLED
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
# define OPTFLOW DISABLED
#endif
#ifndef OPTFLOW_ORIENTATION
# define OPTFLOW_ORIENTATION AP_OPTICALFLOW_ADNS3080_PINS_FORWARD

136
ArduCopter/leds.pde
Unescape View File

@ -1,3 +1,5 @@ @@ -1,3 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void update_lights()
{
switch(led_mode) {
@ -20,9 +22,9 @@ static void update_GPS_light(void) @@ -20,9 +22,9 @@ static void update_GPS_light(void)
case (2):
if(ap.home_is_set) { // JLN update
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
digitalWriteFast(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);
digitalWriteFast(C_LED_PIN, LED_OFF);
}
break;
@ -30,16 +32,16 @@ static void update_GPS_light(void) @@ -30,16 +32,16 @@ static void update_GPS_light(void)
if (g_gps->valid_read == true) {
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);
digitalWriteFast(C_LED_PIN, LED_OFF);
}else{
digitalWrite(C_LED_PIN, LED_ON);
digitalWriteFast(C_LED_PIN, LED_ON);
}
g_gps->valid_read = false;
}
break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
digitalWriteFast(C_LED_PIN, LED_OFF);
break;
}
}
@ -51,14 +53,14 @@ static void update_motor_light(void) @@ -51,14 +53,14 @@ static void update_motor_light(void)
// blink
if(ap_system.motor_light) {
digitalWrite(A_LED_PIN, LED_ON);
digitalWriteFast(A_LED_PIN, LED_ON);
}else{
digitalWrite(A_LED_PIN, LED_OFF);
digitalWriteFast(A_LED_PIN, LED_OFF);
}
}else{
if(!ap_system.motor_light) {
ap_system.motor_light = true;
digitalWrite(A_LED_PIN, LED_ON);
digitalWriteFast(A_LED_PIN, LED_ON);
}
}
}
@ -73,26 +75,26 @@ static void dancing_light() @@ -73,26 +75,26 @@ static void dancing_light()
switch(step)
{
case 0:
digitalWrite(C_LED_PIN, LED_OFF);
digitalWrite(A_LED_PIN, LED_ON);
digitalWriteFast(C_LED_PIN, LED_OFF);
digitalWriteFast(A_LED_PIN, LED_ON);
break;
case 1:
digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(B_LED_PIN, LED_ON);
digitalWriteFast(A_LED_PIN, LED_OFF);
digitalWriteFast(B_LED_PIN, LED_ON);
break;
case 2:
digitalWrite(B_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_ON);
digitalWriteFast(B_LED_PIN, LED_OFF);
digitalWriteFast(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);
digitalWriteFast(A_LED_PIN, LED_OFF);
digitalWriteFast(B_LED_PIN, LED_OFF);
digitalWriteFast(C_LED_PIN, LED_OFF);
ap_system.motor_light = false;
led_mode = NORMAL_LEDS;
}
@ -191,56 +193,56 @@ static void update_copter_leds(void) @@ -191,56 +193,56 @@ static void update_copter_leds(void)
}
static void copter_leds_reset(void) {
digitalWrite(COPTER_LED_1, COPTER_LED_OFF);
digitalWrite(COPTER_LED_2, COPTER_LED_OFF);
digitalWrite(COPTER_LED_3, COPTER_LED_OFF);
digitalWrite(COPTER_LED_4, COPTER_LED_OFF);
digitalWrite(COPTER_LED_5, COPTER_LED_OFF);
digitalWrite(COPTER_LED_6, COPTER_LED_OFF);
digitalWrite(COPTER_LED_7, COPTER_LED_OFF);
digitalWrite(COPTER_LED_8, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
}
static void copter_leds_on(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_1, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWrite(COPTER_LED_2, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
}
#else
digitalWrite(COPTER_LED_2, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
#endif
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_3, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
}
digitalWrite(COPTER_LED_4, COPTER_LED_ON);
digitalWrite(COPTER_LED_5, COPTER_LED_ON);
digitalWrite(COPTER_LED_6, COPTER_LED_ON);
digitalWrite(COPTER_LED_7, COPTER_LED_ON);
digitalWrite(COPTER_LED_8, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_5, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_6, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_7, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_8, COPTER_LED_ON);
}
static void copter_leds_off(void) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_1, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWrite(COPTER_LED_2, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
}
#else
digitalWrite(COPTER_LED_2, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
#endif
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_3, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
}
digitalWrite(COPTER_LED_4, COPTER_LED_OFF);
digitalWrite(COPTER_LED_5, COPTER_LED_OFF);
digitalWrite(COPTER_LED_6, COPTER_LED_OFF);
digitalWrite(COPTER_LED_7, COPTER_LED_OFF);
digitalWrite(COPTER_LED_8, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
}
static void copter_leds_slow_blink(void) {
@ -271,42 +273,42 @@ static void copter_leds_oscillate(void) { @@ -271,42 +273,42 @@ static void copter_leds_oscillate(void) {
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_1, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWrite(COPTER_LED_2, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
}
#else
digitalWrite(COPTER_LED_2, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
#endif
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_3, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
}
digitalWrite(COPTER_LED_4, COPTER_LED_OFF);
digitalWrite(COPTER_LED_5, COPTER_LED_ON);
digitalWrite(COPTER_LED_6, COPTER_LED_ON);
digitalWrite(COPTER_LED_7, COPTER_LED_OFF);
digitalWrite(COPTER_LED_8, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_5, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_6, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
if ( !bitRead(g.copter_leds_mode, 2) ) {
digitalWrite(COPTER_LED_1, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
}
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
if ( !bitRead(g.copter_leds_mode, 3) ) {
digitalWrite(COPTER_LED_2, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
}
#else
digitalWrite(COPTER_LED_2, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
#endif
if ( !bitRead(g.copter_leds_mode, 1) ) {
digitalWrite(COPTER_LED_3, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
}
digitalWrite(COPTER_LED_4, COPTER_LED_ON);
digitalWrite(COPTER_LED_5, COPTER_LED_OFF);
digitalWrite(COPTER_LED_6, COPTER_LED_OFF);
digitalWrite(COPTER_LED_7, COPTER_LED_ON);
digitalWrite(COPTER_LED_8, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_7, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_8, COPTER_LED_ON);
}
else copter_leds_motor_blink = 0; // start blink cycle again
}
@ -314,11 +316,11 @@ static void copter_leds_oscillate(void) { @@ -314,11 +316,11 @@ static void copter_leds_oscillate(void) {
static void copter_leds_GPS_on(void) {
digitalWrite(COPTER_LED_3, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
}
static void copter_leds_GPS_off(void) {
digitalWrite(COPTER_LED_3, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
}
static void copter_leds_GPS_slow_blink(void) {
@ -345,19 +347,19 @@ static void copter_leds_GPS_fast_blink(void) { @@ -345,19 +347,19 @@ static void copter_leds_GPS_fast_blink(void) {
}
static void copter_leds_aux_off(void){
digitalWrite(COPTER_LED_1, COPTER_LED_OFF);
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
}
static void copter_leds_aux_on(void){
digitalWrite(COPTER_LED_1, COPTER_LED_ON);
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
}
void piezo_on(){
digitalWrite(PIEZO_PIN,HIGH);
digitalWriteFast(PIEZO_PIN,HIGH);
}
void piezo_off(){
digitalWrite(PIEZO_PIN,LOW);
digitalWriteFast(PIEZO_PIN,LOW);
}
void piezo_beep(){ // Note! This command should not be used in time sensitive loops

50
ArduCopter/perf_info.pde
Unescape View File

@ -0,0 +1,50 @@ @@ -0,0 +1,50 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// high level performance monitoring
//
// we measure the main loop time
//
#define PERF_INFO_OVERTIME_THRESHOLD_MICROS 10500
uint16_t perf_info_loop_count;
uint32_t perf_info_max_time;
uint16_t perf_info_long_running;
// perf_info_reset - reset all records of loop time to zero
void perf_info_reset()
{
perf_info_loop_count = 0;
perf_info_max_time = 0;
perf_info_long_running = 0;
}
// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold
void perf_info_check_loop_time(uint32_t time_in_micros)
{
perf_info_loop_count++;
if( time_in_micros > perf_info_max_time) {
perf_info_max_time = time_in_micros;
}
if( time_in_micros > PERF_INFO_OVERTIME_THRESHOLD_MICROS ) {
perf_info_long_running++;
}
}
// perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops
uint32_t perf_info_get_num_loops()
{
return perf_info_loop_count;
}
// perf_info_get_max_time - return maximum loop time (in microseconds)
uint32_t perf_info_get_max_time()
{
return perf_info_max_time;
}
// perf_info_get_num_long_running - get number of long running loops
uint32_t perf_info_get_num_long_running()
{
return perf_info_long_running;
}

4
ArduCopter/sensors.pde
Unescape View File

@ -50,7 +50,7 @@ static void init_compass() @@ -50,7 +50,7 @@ static void init_compass()
static void init_optflow()
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
if( optflow.init(false, &timer_scheduler, &spi_semaphore, &spi3_semaphore) == false ) {
g.optflow_enabled = false;
Serial.print_P(PSTR("\nFailed to Init OptFlow "));
@ -65,7 +65,7 @@ static void init_optflow() @@ -65,7 +65,7 @@ static void init_optflow()
// resume timer
timer_scheduler.resume_timer();
#endif
#endif // OPTFLOW == ENABLED
}
// read_battery - check battery voltage and current and invoke failsafe if necessary

8
ArduCopter/setup.pde
Unescape View File

@ -715,7 +715,7 @@ static void clear_offsets() @@ -715,7 +715,7 @@ static void clear_offsets()
static int8_t
setup_optflow(uint8_t argc, const Menu::arg *argv)
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.optflow_enabled = true;
init_optflow();
@ -731,7 +731,7 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) @@ -731,7 +731,7 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
g.optflow_enabled.save();
report_optflow();
#endif
#endif // OPTFLOW == ENABLED
return 0;
}
@ -857,7 +857,7 @@ static void report_flight_modes() @@ -857,7 +857,7 @@ static void report_flight_modes()
void report_optflow()
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
Serial.printf_P(PSTR("OptFlow\n"));
print_divider();
@ -868,7 +868,7 @@ void report_optflow() @@ -868,7 +868,7 @@ void report_optflow()
// degrees(g.optflow_fov));
print_blanks(2);
#endif
#endif // OPTFLOW == ENABLED
}
#if FRAME_CONFIG == HELI_FRAME

6
ArduCopter/system.pde
Unescape View File

@ -62,7 +62,7 @@ static void init_ardupilot() @@ -62,7 +62,7 @@ static void init_ardupilot()
// USB_MUX_PIN
pinMode(USB_MUX_PIN, INPUT);
ap_system.usb_connected = !digitalRead(USB_MUX_PIN);
ap_system.usb_connected = !digitalReadFast(USB_MUX_PIN);
if (!ap_system.usb_connected) {
// USB is not connected, this means UART0 may be a Xbee, with
// its darned bricking problem. We can't write to it for at
@ -612,7 +612,7 @@ static void update_throttle_cruise(int16_t tmp) @@ -612,7 +612,7 @@ static void update_throttle_cruise(int16_t tmp)
static boolean
check_startup_for_CLI()
{
return (digitalRead(SLIDE_SWITCH_PIN) == 0);
return (digitalReadFast(SLIDE_SWITCH_PIN) == 0);
}
#endif // CLI_ENABLED
@ -639,7 +639,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) @@ -639,7 +639,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
#if USB_MUX_PIN > 0
static void check_usb_mux(void)
{
bool usb_check = !digitalRead(USB_MUX_PIN);
bool usb_check = !digitalReadFast(USB_MUX_PIN);
if (usb_check == ap_system.usb_connected) {
return;
}

4
ArduCopter/test.pde
Unescape View File

@ -935,7 +935,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) @@ -935,7 +935,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
static int8_t
test_optflow(uint8_t argc, const Menu::arg *argv)
{
#ifdef OPTFLOW_ENABLED
#if OPTFLOW == ENABLED
if(g.optflow_enabled) {
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
print_hit_enter();
@ -964,7 +964,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv) @@ -964,7 +964,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
#else
print_test_disabled();
return (0);
#endif
#endif // OPTFLOW == ENABLED
}

Write Preview
Loading…
Cancel
Save