yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Support new mission status

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 18:37:19 +02:00
parent 2cf10a5e99
commit a4b2389460
3 changed files with 61 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/commander/commander.cpp
View File

@@ -190,6 +190,8 @@ static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static unsigned _last_mission_instance = 0;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -839,7 +841,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
//Play tune first time we initialize HOME
if (!status.condition_home_position_valid) {
tune_positive(true);
tune_home_set(true);
}
/* mark home position as set */
@@ -1764,6 +1766,28 @@ int commander_thread_main(int argc, char *argv[])
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
/* Only evaluate mission state if home is set,
* this prevents false positives for the mission
* rejection. Back off 2 seconds to not overlay
* home tune.
*/
if (status.condition_home_position_valid &&
(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
_last_mission_instance != mission_result.instance_count) {
if (mission_result.valid) {
/* the mission is valid */
tune_mission_ok(true);
warnx("mission ok");
} else {
/* the mission is not valid */
tune_mission_fail(true);
warnx("mission fail");
}
/* prevent further feedback until the mission changes */
_last_mission_instance = mission_result.instance_count;
}
/* RC input check */
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {

33
src/modules/commander/commander_helper.cpp
View File

@@ -172,6 +172,39 @@ void set_tune(int tune)
}
}
void tune_home_set(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
}
void tune_mission_ok(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
}
void tune_mission_fail(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
}
}
/**
* Blink green LED and play positive tune (if use_buzzer == true).
*/

3
src/modules/commander/commander_helper.h
View File

@@ -58,6 +58,9 @@ void buzzer_deinit(void);
void set_tune_override(int tune);
void set_tune(int tune);
void tune_home_set(bool use_buzzer);
void tune_mission_ok(bool use_buzzer);
void tune_mission_fail(bool use_buzzer);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
void tune_negative(bool use_buzzer);