aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: replaced digitalRead and digitalWrite with faster calls

Browse Source 
improved performance logging to dataflash
This commit is contained in:
rmackay9 2012-11-19 01:16:07 +09:00
parent 67fadd337d
commit 07a7a1acd8
10 changed files with 170 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduCopter/ArduCopter.pde
View File

@ -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;
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);
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()
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()
// 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()
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()
}
}
// 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()
// 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()
}
// 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)
}
}
}
#endif
#endif // OPTFLOW == ENABLED
// called at 50hz
static void update_GPS(void)

8
ArduCopter/Attitude.pde
View File

@ -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)
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)
return input_pitch+of_pitch;
#else
return input_pitch;
#endif
#endif // OPTFLOW == ENABLED
}

23
ArduCopter/Log.pde
View File

@ -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()
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()
temp7,
(long)temp8,
(long)temp9);
#endif
}
// Write an Nav Tuning packet. Total length : 24 bytes
@ -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()
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
View File

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

@ -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)
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)
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)
// 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()
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)
}
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) {
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) {
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) {
}
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 Normal file
View File

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

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

@ -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)
g.optflow_enabled.save();
report_optflow();
#endif
#endif // OPTFLOW == ENABLED
return 0;
}
@ -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()
// degrees(g.optflow_fov));
print_blanks(2);
#endif
#endif // OPTFLOW == ENABLED
}
#if FRAME_CONFIG == HELI_FRAME

6
ArduCopter/system.pde
View File

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

@ -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)
#else
print_test_disabled();
return (0);
#endif
#endif // OPTFLOW == ENABLED
}