Merge pull request #626 from PX4/rc_mapping

Improved RC calibration behaviour, fully supported setting trim offsets
This commit is contained in:
Lorenz Meier 2014-02-09 15:49:55 +01:00
commit f52f15c791
6 changed files with 56 additions and 17 deletions

View File

@ -214,7 +214,7 @@ 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);
@ -620,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;
@ -1076,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 */
@ -1470,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);
@ -1479,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);
@ -1493,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)
@ -1512,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();
}
}
}
@ -1795,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 */
@ -1806,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)

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()

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);

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;
}

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_ */

View File

@ -206,6 +206,7 @@ struct vehicle_status_s
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;