Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

some changes in structure

Browse Source
This commit is contained in:
Marco Bauer
2013-01-13 13:50:07 +01:00
parent 8ec566d0cb
commit edc3ae7b6c
2 changed files with 143 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+2 -1
View File
@@ -109,7 +109,8 @@ ifeq ($(SYSTYPE),Linux)
SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
endif endif
ifeq ($(SERIAL_PORTS),) ifeq ($(SERIAL_PORTS),)
SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" #SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
SERIAL_PORTS = "\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
endif endif
upload: $(FIRMWARE_BUNDLE) $(UPLOADER) upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
apps/drivers/blinkm/blinkm.cpp
+141 -154
View File
@@ -149,6 +149,21 @@ private:
MORSE_CODE MORSE_CODE
}; };
enum systemDefines {
LED_OFF,
LED_RED,
LED_YELLOW,
LED_PURPLE,
LED_GREEN,
LED_BLUE,
LED_WHITE,
LED_ONTIME=100,
LED_OFFTIME=100,
LED_NOBLINK=0,
LED_BLINK=1,
MAX_CELL_VOLTAGE=43
};
work_s _work; work_s _work;
static int led_color_1; static int led_color_1;
@@ -159,7 +174,7 @@ private:
static int led_color_6; static int led_color_6;
static int led_blink; static int led_blink;
static int systemstate_run; static bool systemstate_run;
void setLEDColor(int ledcolor); void setLEDColor(int ledcolor);
static void led_trampoline(void *arg); static void led_trampoline(void *arg);
@@ -218,19 +233,6 @@ const char *BlinkM::script_names[] = {
nullptr nullptr
}; };
#define MAX_CELL_VOLTAGE 43 /* cell voltage if full charged */
#define LED_OFF 0
#define LED_RED 1
#define LED_YELLOW 2
#define LED_PURPLE 3
#define LED_GREEN 4
#define LED_BLUE 5
#define LED_WHITE 6
#define LED_ONTIME 100
#define LED_OFFTIME 100
#define LED_BLINK 1
#define LED_NOBLINK 0
int BlinkM::led_color_1 = LED_OFF; int BlinkM::led_color_1 = LED_OFF;
int BlinkM::led_color_2 = LED_OFF; int BlinkM::led_color_2 = LED_OFF;
@@ -240,7 +242,8 @@ int BlinkM::led_color_5 = LED_OFF;
int BlinkM::led_color_6 = LED_OFF; int BlinkM::led_color_6 = LED_OFF;
int BlinkM::led_blink = LED_NOBLINK; int BlinkM::led_blink = LED_NOBLINK;
int BlinkM::systemstate_run = 0; bool BlinkM::systemstate_run = false;
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
@@ -281,28 +284,26 @@ int
BlinkM::setMode(int mode) BlinkM::setMode(int mode)
{ {
if(mode == 1) { if(mode == 1) {
if(BlinkM::systemstate_run == 0) { if(BlinkM::systemstate_run == false) {
/* set some sensible defaults */ /* set some sensible defaults */
set_fade_speed(255); set_fade_speed(255);
/* turn off by default */ /* turn off by default */
play_script(BLACK); play_script(BLACK);
set_fade_speed(255);
stop_script(); stop_script();
set_rgb(0,0,0); //set_rgb(0,0,0);
BlinkM::systemstate_run = 1; BlinkM::systemstate_run = true;
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
} }
} else { } else {
BlinkM::systemstate_run = 0; BlinkM::systemstate_run = false;
usleep(1000000); usleep(1000000);
/* set some sensible defaults */ /* set some sensible defaults */
set_fade_speed(255); set_fade_speed(255);
/* turn off by default */ /* turn off by default */
play_script(BLACK); play_script(BLACK);
set_fade_speed(255);
stop_script(); stop_script();
set_rgb(0,0,0); //set_rgb(0,0,0);
} }
return OK; return OK;
@@ -403,7 +404,6 @@ BlinkM::led()
int system_voltage = 0; int system_voltage = 0;
int num_of_used_sats = 0; int num_of_used_sats = 0;
int poll_ret;
if(!topic_initialized) { if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
@@ -415,12 +415,6 @@ BlinkM::led()
topic_initialized = true; topic_initialized = true;
} }
pollfd fds[2];
fds[0].fd = vehicle_status_sub_fd;
fds[0].events = POLLIN;
fds[1].fd = vehicle_gps_position_sub_fd;
fds[1].events = POLLIN;
if(led_thread_ready == true) { if(led_thread_ready == true) {
if(!detected_cells_blinked) { if(!detected_cells_blinked) {
if(num_of_cells > 0) { if(num_of_cells > 0) {
@@ -523,139 +517,132 @@ BlinkM::led()
BlinkM::setLEDColor(LED_OFF); BlinkM::setLEDColor(LED_OFF);
} }
//poll_ret = ::poll(&fds[0],1,1000); /* obtained data for the first file descriptor */
poll_ret = ::poll(fds, 2, 1000); struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
if (poll_ret == 0) { bool new_data;
/* this means none of our providers is giving us data */ orb_check(vehicle_status_sub_fd, &new_data);
printf("[blinkm_systemstate_sensor] Got no data within a second\n");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
log("poll error %d", errno);
usleep(1000000);
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
/* copy sensors raw data into local buffer */
/* vehicle_status */ if (new_data) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
}
/* vehicle_gps_position */ orb_check(vehicle_gps_position_sub_fd, &new_data);
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
/* get actual battery voltage */ if (new_data) {
system_voltage = (int)vehicle_status_raw.voltage_battery*10; orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
}
/* get number of used satellites in navigation */ /* get actual battery voltage */
num_of_used_sats = 0; system_voltage = (int)vehicle_status_raw.voltage_battery*10;
for(int satloop=0; satloop<20; satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
if(num_of_cells == 0) { /* get number of used satellites in navigation */
/* looking for lipo cells that are connected */ num_of_used_sats = 0;
printf("<blinkm> checking cells\n"); for(int satloop=0; satloop<20; satloop++) {
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; num_of_used_sats++;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
/* LED Pattern for battery low warning */
BlinkM::led_color_1 = LED_YELLOW;
BlinkM::led_color_2 = LED_YELLOW;
BlinkM::led_color_3 = LED_YELLOW;
BlinkM::led_color_4 = LED_YELLOW;
BlinkM::led_color_5 = LED_YELLOW;
BlinkM::led_color_6 = LED_YELLOW;
BlinkM::led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
/* LED Pattern for battery critical alerting */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_RED;
BlinkM::led_color_5 = LED_RED;
BlinkM::led_color_6 = LED_RED;
BlinkM::led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_system_armed == false) {
/* system not armed */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_RED;
BlinkM::led_color_5 = LED_RED;
BlinkM::led_color_6 = LED_RED;
BlinkM::led_blink = LED_NOBLINK;
} else {
/* armed system - initial led pattern */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_OFF;
BlinkM::led_color_5 = LED_OFF;
BlinkM::led_color_6 = LED_OFF;
BlinkM::led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
BlinkM::led_color_4 = LED_OFF;
break;
case VEHICLE_FLIGHT_MODE_STAB:
BlinkM::led_color_4 = LED_YELLOW;
break;
case VEHICLE_FLIGHT_MODE_HOLD:
BlinkM::led_color_4 = LED_BLUE;
break;
case VEHICLE_FLIGHT_MODE_AUTO:
BlinkM::led_color_4 = LED_GREEN;
break;
}
/* handling used sat´s */
if(num_of_used_sats >= 7) {
BlinkM::led_color_1 = LED_OFF;
BlinkM::led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) {
BlinkM::led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) {
BlinkM::led_color_3 = LED_OFF;
}
}
}
}
/*
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
vehicle_status_raw.voltage_battery,
vehicle_status_raw.flag_system_armed,
vehicle_status_raw.flight_mode,
num_of_cells,
vehicle_status_raw.battery_warning,
num_of_used_sats,
vehicle_gps_position_raw.fix_type,
vehicle_gps_position_raw.satellites_visible);
*/
} }
} }
if(num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(system_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
/* LED Pattern for battery low warning */
BlinkM::led_color_1 = LED_YELLOW;
BlinkM::led_color_2 = LED_YELLOW;
BlinkM::led_color_3 = LED_YELLOW;
BlinkM::led_color_4 = LED_YELLOW;
BlinkM::led_color_5 = LED_YELLOW;
BlinkM::led_color_6 = LED_YELLOW;
BlinkM::led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
/* LED Pattern for battery critical alerting */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_RED;
BlinkM::led_color_5 = LED_RED;
BlinkM::led_color_6 = LED_RED;
BlinkM::led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_system_armed == false) {
/* system not armed */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_RED;
BlinkM::led_color_5 = LED_RED;
BlinkM::led_color_6 = LED_RED;
BlinkM::led_blink = LED_NOBLINK;
} else {
/* armed system - initial led pattern */
BlinkM::led_color_1 = LED_RED;
BlinkM::led_color_2 = LED_RED;
BlinkM::led_color_3 = LED_RED;
BlinkM::led_color_4 = LED_OFF;
BlinkM::led_color_5 = LED_OFF;
BlinkM::led_color_6 = LED_OFF;
BlinkM::led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
BlinkM::led_color_4 = LED_OFF;
break;
case VEHICLE_FLIGHT_MODE_STAB:
BlinkM::led_color_4 = LED_YELLOW;
break;
case VEHICLE_FLIGHT_MODE_HOLD:
BlinkM::led_color_4 = LED_BLUE;
break;
case VEHICLE_FLIGHT_MODE_AUTO:
BlinkM::led_color_4 = LED_GREEN;
break;
}
/* handling used sat´s */
if(num_of_used_sats >= 7) {
BlinkM::led_color_1 = LED_OFF;
BlinkM::led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) {
BlinkM::led_color_2 = LED_OFF;
BlinkM::led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) {
BlinkM::led_color_3 = LED_OFF;
}
}
}
}
/*
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tBattWarn:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
vehicle_status_raw.voltage_battery,
vehicle_status_raw.flag_system_armed,
vehicle_status_raw.flight_mode,
num_of_cells,
vehicle_status_raw.battery_warning,
num_of_used_sats,
vehicle_gps_position_raw.fix_type,
vehicle_gps_position_raw.satellites_visible);
*/
led_thread_runcount=1; led_thread_runcount=1;
led_thread_ready = true; led_thread_ready = true;
led_interval = LED_OFFTIME; led_interval = LED_OFFTIME;
@@ -674,7 +661,7 @@ BlinkM::led()
break; break;
} }
if(BlinkM::systemstate_run == 1) { if(BlinkM::systemstate_run == true) {
/* re-queue ourselves to run again later */ /* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} }