Skip to content

Commit 7cf2255

Browse files
rmackay9rmackay9
committed
Copter LEDs: replace bitRead with bitmask
Consolidate all checks of led_mode to leds.pde Add #defines for bitmasks comparisons Some formatting changes
1 parent 6816c45 commit 7cf2255

File tree

5 files changed

+111
-91
lines changed

5 files changed

+111
-91
lines changed

‎ArduCopter/compat.h

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,5 @@
1111
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
1212
static void run_cli(AP_HAL::UARTDriver *port);
1313

14-
// XXX AP_HAL tofix - what is bitRead? temporarily disable.
15-
#define bitRead(a,b) false
16-
1714
#endif // __COMPAT_H__
1815

‎ArduCopter/events.pde

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -98,9 +98,8 @@ static void low_battery_event(void)
9898
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
9999

100100
#if COPTER_LEDS == ENABLED
101-
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only
102-
piezo_on();
103-
}
101+
// Only Activate if a battery is connected to avoid alarm on USB only
102+
piezo_on();
104103
#endif // COPTER_LEDS
105104
}
106105

‎ArduCopter/leds.pde

Lines changed: 106 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -119,78 +119,104 @@ static void clear_leds()
119119
// Piezo Enables Tone on reaching low battery or current alert
120120
/////////////////////////////////////////////////////////////////////////////////////////////
121121

122+
#define COPTER_LEDS_BITMASK_ENABLED 0x01 // bit #0
123+
#define COPTER_LEDS_BITMASK_GPS 0x02 // bit #1
124+
#define COPTER_LEDS_BITMASK_AUX 0x04 // bit #2
125+
#define COPTER_LEDS_BITMASK_BEEPER 0x08 // bit #3
126+
#define COPTER_LEDS_BITMASK_BATT_OSCILLATE 0x10 // bit #4
127+
#define COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK 0x20 // bit #5
128+
#define COPTER_LEDS_BITMASK_GPS_NAV_BLINK 0x40 // bit #6
122129

123130
#if COPTER_LEDS == ENABLED
131+
static void copter_leds_init(void)
132+
{
133+
pinMode(COPTER_LED_1, OUTPUT); //Motor LED
134+
pinMode(COPTER_LED_2, OUTPUT); //Motor LED
135+
pinMode(COPTER_LED_3, OUTPUT); //Motor LED
136+
pinMode(COPTER_LED_4, OUTPUT); //Motor LED
137+
pinMode(COPTER_LED_5, OUTPUT); //Motor or Aux LED
138+
pinMode(COPTER_LED_6, OUTPUT); //Motor or Aux LED
139+
pinMode(COPTER_LED_7, OUTPUT); //Motor or GPS LED
140+
pinMode(COPTER_LED_8, OUTPUT); //Motor or GPS LED
141+
142+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
143+
piezo_beep();
144+
}
145+
}
146+
124147
static void update_copter_leds(void)
125148
{
126149
if (g.copter_leds_mode == 0) {
127150
copter_leds_reset(); //method of reintializing LED state
128151
}
129152

130-
if ( bitRead(g.copter_leds_mode, 0) ) {
131-
if (motors.armed() == true) {
132-
if (ap.low_battery == true) {
133-
if ( bitRead(g.copter_leds_mode, 4 )) {
153+
// motor leds control
154+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) {
155+
if (motors.armed()) {
156+
if (ap.low_battery) {
157+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BATT_OSCILLATE) {
134158
copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink
135159
} else {
136160
copter_leds_fast_blink(); //if motors are armed, but battery level is low, motor leds oscillate
137161
}
138-
} else if ( !bitRead(g.copter_leds_mode, 5 ) ) {
139-
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
140-
} else if ( bitRead(g.copter_leds_mode, 5 ) ) {
141-
if ( copter_leds_nav_blink >0 ) {
142-
copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
162+
} else {
163+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK) {
164+
if (copter_leds_nav_blink > 0) {
165+
copter_leds_slow_blink(); //if nav command was seen, blink LEDs.
166+
} else {
167+
copter_leds_on();
168+
}
143169
} else {
144-
copter_leds_on();
170+
copter_leds_on(); //if motors are armed, battery level OK, all motor leds ON
145171
}
146172
}
147173
} else {
148174
copter_leds_slow_blink(); //if motors are not armed, blink motor leds
149175
}
150176
}
151177

152-
if ( bitRead(g.copter_leds_mode, 1) ) {
178+
// GPS led control
179+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) {
153180

154181
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
155182
// ---------------------------------------------------------------------
156183
switch (g_gps->status()) {
157184

158-
case (2):
185+
case GPS::NO_GPS:
186+
copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off
187+
break;
188+
189+
case GPS::NO_FIX:
190+
copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow
191+
break;
192+
193+
case GPS::GPS_OK_FIX_2D:
194+
case GPS::GPS_OK_FIX_3D:
159195
if(ap.home_is_set) {
160-
if ( !bitRead(g.copter_leds_mode, 6 ) ) {
161-
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
162-
} else if (bitRead(g.copter_leds_mode, 6 ) ) {
163-
if ( copter_leds_nav_blink >0 ) {
196+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) {
197+
if (copter_leds_nav_blink >0) {
164198
copter_leds_GPS_slow_blink(); //if nav command was seen, blink LEDs.
165199
} else {
166200
copter_leds_GPS_on();
167201
}
202+
} else {
203+
copter_leds_GPS_on(); //Turn GPS LEDs on when gps has valid fix AND home is set
168204
}
169205
} else {
170206
copter_leds_GPS_fast_blink(); //if GPS has fix, but home is not set, blink GPS LED fast
171207
}
172208
break;
173-
174-
case (1):
175-
176-
copter_leds_GPS_slow_blink(); //if GPS has valid reads, but no fix, blink GPS LED slow
177-
178-
break;
179-
180-
default:
181-
copter_leds_GPS_off(); //if no valid GPS signal, turn GPS LED off
182-
break;
183209
}
184210
}
185211

186-
if ( bitRead(g.copter_leds_mode, 2) ) {
187-
if (200 <= g.rc_7.control_in && g.rc_7.control_in < 400) {
212+
// AUX led control
213+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX) {
214+
if (ap_system.CH7_flag) {
188215
copter_leds_aux_on(); //if sub-control of Ch7 is high, turn Aux LED on
189-
} else if (g.rc_7.control_in < 200) {
216+
} else {
190217
copter_leds_aux_off(); //if sub-control of Ch7 is low, turn Aux LED off
191218
}
192219
}
193-
194220
}
195221

196222
static void copter_leds_reset(void) {
@@ -205,17 +231,17 @@ static void copter_leds_reset(void) {
205231
}
206232

207233
static void copter_leds_on(void) {
208-
if ( !bitRead(g.copter_leds_mode, 2) ) {
234+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
209235
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
210236
}
211237
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
212-
if ( !bitRead(g.copter_leds_mode, 3) ) {
238+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
213239
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
214240
}
215241
#else
216242
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
217243
#endif
218-
if ( !bitRead(g.copter_leds_mode, 1) ) {
244+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) {
219245
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
220246
}
221247
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
@@ -226,17 +252,17 @@ static void copter_leds_on(void) {
226252
}
227253

228254
static void copter_leds_off(void) {
229-
if ( !bitRead(g.copter_leds_mode, 2) ) {
255+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
230256
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
231257
}
232258
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
233-
if ( !bitRead(g.copter_leds_mode, 3) ) {
259+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
234260
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
235261
}
236262
#else
237263
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
238264
#endif
239-
if ( !bitRead(g.copter_leds_mode, 1) ) {
265+
if (!(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) {
240266
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
241267
}
242268
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
@@ -247,43 +273,47 @@ static void copter_leds_off(void) {
247273
}
248274

249275
static void copter_leds_slow_blink(void) {
250-
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
251-
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds
276+
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
277+
278+
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds
252279
copter_leds_off();
253-
if ( bitRead(g.copter_leds_mode, 5 ) && !bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker...
254-
copter_leds_nav_blink--; // decrement the Nav Blink counter
280+
281+
// if blinking is called by the Nav Blinker decrement the Nav Blink counter
282+
if ((g.copter_leds_mode & COPTER_LEDS_BITMASK_MOTOR_NAV_BLINK) && !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) && copter_leds_nav_blink >0 ) {
283+
copter_leds_nav_blink--;
255284
}
256285
}else if (5 < copter_leds_motor_blink && copter_leds_motor_blink < 11) {
257286
copter_leds_on();
287+
}else{
288+
copter_leds_motor_blink = 0; // start blink cycle again
258289
}
259-
else copter_leds_motor_blink = 0; // start blink cycle again
260290
}
261291

262-
static void copter_leds_fast_blink(void) {
263-
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
264-
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
292+
static void copter_leds_fast_blink(void) {
293+
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
294+
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
265295
copter_leds_on();
266296
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
267297
copter_leds_off();
298+
}else{
299+
copter_leds_motor_blink = 0; // start blink cycle again
268300
}
269-
else copter_leds_motor_blink = 0; // start blink cycle again
270301
}
271302

272-
273303
static void copter_leds_oscillate(void) {
274-
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
275-
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
276-
if ( !bitRead(g.copter_leds_mode, 2) ) {
304+
copter_leds_motor_blink++; // this increments once every 1/10 second because it is in the 10hz loop
305+
if ( 0 < copter_leds_motor_blink && copter_leds_motor_blink < 3 ) { // when the counter reaches 3 (1/5 sec), then toggle the leds
306+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
277307
digitalWriteFast(COPTER_LED_1, COPTER_LED_ON);
278308
}
279309
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
280-
if ( !bitRead(g.copter_leds_mode, 3) ) {
310+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
281311
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
282312
}
283313
#else
284314
digitalWriteFast(COPTER_LED_2, COPTER_LED_ON);
285315
#endif
286-
if ( !bitRead(g.copter_leds_mode, 1) ) {
316+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS)) {
287317
digitalWriteFast(COPTER_LED_3, COPTER_LED_OFF);
288318
}
289319
digitalWriteFast(COPTER_LED_4, COPTER_LED_OFF);
@@ -292,30 +322,29 @@ static void copter_leds_oscillate(void) {
292322
digitalWriteFast(COPTER_LED_7, COPTER_LED_OFF);
293323
digitalWriteFast(COPTER_LED_8, COPTER_LED_OFF);
294324
}else if (2 < copter_leds_motor_blink && copter_leds_motor_blink < 5) {
295-
if ( !bitRead(g.copter_leds_mode, 2) ) {
325+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_AUX)) {
296326
digitalWriteFast(COPTER_LED_1, COPTER_LED_OFF);
297327
}
298328
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
299-
if ( !bitRead(g.copter_leds_mode, 3) ) {
329+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER)) {
300330
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
301331
}
302332
#else
303333
digitalWriteFast(COPTER_LED_2, COPTER_LED_OFF);
304334
#endif
305-
if ( !bitRead(g.copter_leds_mode, 1) ) {
335+
if ( !(g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS) ) {
306336
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
307337
}
308338
digitalWriteFast(COPTER_LED_4, COPTER_LED_ON);
309339
digitalWriteFast(COPTER_LED_5, COPTER_LED_OFF);
310340
digitalWriteFast(COPTER_LED_6, COPTER_LED_OFF);
311341
digitalWriteFast(COPTER_LED_7, COPTER_LED_ON);
312342
digitalWriteFast(COPTER_LED_8, COPTER_LED_ON);
343+
}else{
344+
copter_leds_motor_blink = 0; // start blink cycle again
313345
}
314-
else copter_leds_motor_blink = 0; // start blink cycle again
315346
}
316347

317-
318-
319348
static void copter_leds_GPS_on(void) {
320349
digitalWriteFast(COPTER_LED_3, COPTER_LED_ON);
321350
}
@@ -328,7 +357,7 @@ static void copter_leds_GPS_slow_blink(void) {
328357
copter_leds_GPS_blink++; // this increments once every 1/10 second because it is in the 10hz loop
329358
if ( 0 < copter_leds_GPS_blink && copter_leds_GPS_blink < 6 ) { // when the counter reaches 5 (1/2 sec), then toggle the leds
330359
copter_leds_GPS_off();
331-
if ( bitRead(g.copter_leds_mode, 6 ) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker...
360+
if ((g.copter_leds_mode & COPTER_LEDS_BITMASK_GPS_NAV_BLINK) && copter_leds_nav_blink >0 ) { // if blinking is called by the Nav Blinker...
332361
copter_leds_nav_blink--; // decrement the Nav Blink counter
333362
}
334363
}else if (5 < copter_leds_GPS_blink && copter_leds_GPS_blink < 11) {
@@ -356,17 +385,31 @@ static void copter_leds_aux_on(void){
356385
}
357386

358387
void piezo_on(){
359-
digitalWriteFast(PIEZO_PIN,HIGH);
388+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
389+
digitalWriteFast(PIEZO_PIN,HIGH);
390+
}
360391
}
361392

362393
void piezo_off(){
363-
digitalWriteFast(PIEZO_PIN,LOW);
394+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
395+
digitalWriteFast(PIEZO_PIN,LOW);
396+
}
364397
}
365398

366399
void piezo_beep(){ // Note! This command should not be used in time sensitive loops
367-
piezo_on();
368-
delay(100);
369-
piezo_off();
400+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
401+
piezo_on();
402+
delay(100);
403+
piezo_off();
404+
}
405+
}
406+
407+
void piezo_beep_twice(){ // Note! This command should not be used in time sensitive loops
408+
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BEEPER) {
409+
piezo_beep();
410+
delay(50);
411+
piezo_beep();
412+
}
370413
}
371414

372415
#endif //COPTER_LEDS

‎ArduCopter/motors.pde

Lines changed: 2 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -120,11 +120,7 @@ static void init_arm_motors()
120120
}
121121

122122
#if COPTER_LEDS == ENABLED
123-
if ( bitRead(g.copter_leds_mode, 3) ) {
124-
piezo_beep();
125-
delay(50);
126-
piezo_beep();
127-
}
123+
piezo_beep_twice();
128124
#endif
129125

130126
// Remember Orientation
@@ -191,9 +187,7 @@ static void init_disarm_motors()
191187
set_takeoff_complete(false);
192188

193189
#if COPTER_LEDS == ENABLED
194-
if ( bitRead(g.copter_leds_mode, 3) ) {
195-
piezo_beep();
196-
}
190+
piezo_beep();
197191
#endif
198192

199193
// setup fast AHRS gains to get right attitude

‎ArduCopter/system.pde

Lines changed: 1 addition & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -136,22 +136,9 @@ static void init_ardupilot()
136136
relay.init();
137137

138138
#if COPTER_LEDS == ENABLED
139-
pinMode(COPTER_LED_1, OUTPUT); //Motor LED
140-
pinMode(COPTER_LED_2, OUTPUT); //Motor LED
141-
pinMode(COPTER_LED_3, OUTPUT); //Motor LED
142-
pinMode(COPTER_LED_4, OUTPUT); //Motor LED
143-
pinMode(COPTER_LED_5, OUTPUT); //Motor or Aux LED
144-
pinMode(COPTER_LED_6, OUTPUT); //Motor or Aux LED
145-
pinMode(COPTER_LED_7, OUTPUT); //Motor or GPS LED
146-
pinMode(COPTER_LED_8, OUTPUT); //Motor or GPS LED
147-
148-
if ( !bitRead(g.copter_leds_mode, 3) ) {
149-
piezo_beep();
150-
}
151-
139+
copter_leds_init();
152140
#endif
153141

154-
155142
// load parameters from EEPROM
156143
load_parameters();
157144

0 commit comments

Comments
 (0)