fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 14:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged origin/master into pubsub_cleanup

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-09 16:04:32 +01:00
parent ac326beaaa f52f15c791
commit 773f70a9df
147 changed files with 5405 additions and 4220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+92 -20
View File
@@ -154,6 +154,16 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
@@ -204,12 +214,15 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
void print_reject_mode(const char *msg);
void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
void print_reject_arm(const char *msg);
void print_status();
int arm();
int disarm();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
@@ -277,6 +290,16 @@ int commander_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "arm")) {
arm();
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
disarm();
exit(0);
}
usage("unrecognized command");
exit(1);
}
@@ -344,6 +367,30 @@ void print_status()
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
int arm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
int disarm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@@ -537,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -583,6 +620,8 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
/* armed topic */
orb_advert_t armed_pub;
@@ -1039,7 +1078,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* ignore RC signals if in offboard control mode */
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0 && !status.rc_input_blocked) {
/* start RC input check */
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
@@ -1366,7 +1405,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
warnx("mode sw not finite");
/* default to manual if signal is invalid */
current_status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
@@ -1433,7 +1472,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
break; // changed successfully or already in this state
// else fallback to SEATBELT
print_reject_mode("EASY");
print_reject_mode(current_status, "EASY");
}
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
@@ -1442,7 +1481,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
break; // changed successfully or already in this mode
if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
print_reject_mode("SEATBELT");
print_reject_mode(current_status, "SEATBELT");
// else fallback to MANUAL
res = main_state_transition(current_status, MAIN_STATE_MANUAL);
@@ -1456,7 +1495,7 @@ check_main_state_machine(struct vehicle_status_s *current_status)
break; // changed successfully or already in this state
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode("AUTO");
print_reject_mode(current_status, "AUTO");
res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
@@ -1475,16 +1514,25 @@ check_main_state_machine(struct vehicle_status_s *current_status)
}
void
print_reject_mode(const char *msg)
print_reject_mode(struct vehicle_status_s *current_status, const char *msg)
{
hrt_abstime t = hrt_absolute_time();
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
sprintf(s, "#audio: warning: reject %s", msg);
sprintf(s, "#audio: REJECT %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
// only buzz if armed, because else we're driving people nuts indoors
// they really need to look at the leds as well.
if (current_status->arming_state == ARMING_STATE_ARMED) {
tune_negative();
} else {
// Always show the led indication
led_negative();
}
}
}
@@ -1758,7 +1806,15 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_rc_calibration(mavlink_fd);
/* disable RC control input completely */
status.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_trim_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
@@ -1769,6 +1825,18 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
}
/* this always succeeds */
calib_ret = OK;
}
if (calib_ret == OK)
@@ -1827,6 +1895,10 @@ void *commander_low_prio_loop(void *arg)
break;
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
src/modules/commander/commander_helper.cpp
+6 -1
View File
@@ -123,11 +123,16 @@ void tune_neutral()
}
void tune_negative()
{
led_negative();
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
}
void led_negative()
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
}
int tune_arm()
src/modules/commander/commander_helper.h
+3
View File
@@ -62,6 +62,9 @@ int tune_arm(void);
int tune_low_bat(void);
int tune_critical_bat(void);
void tune_stop(void);
void led_negative();
int blink_msg_state();
int led_init(void);
src/modules/commander/rc_calibration.cpp
+5 -6
View File
@@ -53,17 +53,16 @@
#endif
static const int ERROR = -1;
int do_rc_calibration(int mavlink_fd)
int do_trim_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(200000);
struct manual_control_setpoint_s sp;
bool changed;
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no manual control, aborting");
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
return ERROR;
}
@@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd)
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim calibration done");
mavlink_log_info(mavlink_fd, "trim cal done");
close(sub_man);
return OK;
}
src/modules/commander/rc_calibration.h
+1 -1
View File
@@ -41,6 +41,6 @@
#include <stdint.h>
int do_rc_calibration(int mavlink_fd);
int do_trim_calibration(int mavlink_fd);
#endif /* RC_CALIBRATION_H_ */