forked from Archive/PX4-Autopilot
WIP on mode switching input
This commit is contained in:
Lorenz Meier
parent
c259a34a82
commit
b2068b4e0e
|
|
@ -95,6 +95,9 @@
|
|||
|
||||
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
||||
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
||||
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
extern struct system_load_s system_load;
|
||||
|
|
@ -152,6 +155,7 @@ static int led_on(int led);
|
|||
static int led_off(int led);
|
||||
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
|
||||
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
|
||||
|
||||
|
|
@ -179,7 +183,7 @@ static int buzzer_init()
|
|||
buzzer = open("/dev/tone_alarm", O_WRONLY);
|
||||
|
||||
if (buzzer < 0) {
|
||||
fprintf(stderr, "[commander] Buzzer: open fail\n");
|
||||
fprintf(stderr, "[cmd] Buzzer: open fail\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -197,12 +201,12 @@ static int led_init()
|
|||
leds = open(LED_DEVICE_PATH, 0);
|
||||
|
||||
if (leds < 0) {
|
||||
fprintf(stderr, "[commander] LED: open fail\n");
|
||||
fprintf(stderr, "[cmd] LED: open fail\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
|
||||
fprintf(stderr, "[commander] LED: ioctl fail\n");
|
||||
fprintf(stderr, "[cmd] LED: ioctl fail\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -268,6 +272,30 @@ void tune_confirm(void) {
|
|||
ioctl(buzzer, TONE_SET_ALARM, 3);
|
||||
}
|
||||
|
||||
void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
{
|
||||
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
struct manual_control_setpoint_s sp;
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
|
||||
|
||||
/* set parameters */
|
||||
|
||||
float p = sp.roll;
|
||||
param_set(param_find("TRIM_ROLL"), &p);
|
||||
p = sp.pitch;
|
||||
param_set(param_find("TRIM_PITCH"), &p);
|
||||
p = sp.yaw;
|
||||
param_set(param_find("TRIM_YAW"), &p);
|
||||
|
||||
/* store to permanent storage */
|
||||
/* auto-save to EEPROM */
|
||||
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_info(mavlink_fd, "[cmd] trim calibration done");
|
||||
}
|
||||
|
||||
void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
||||
{
|
||||
|
||||
|
|
@ -310,7 +338,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
};
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] failed to set scale / offsets for mag");
|
||||
}
|
||||
|
||||
/* calibrate range */
|
||||
|
|
@ -358,7 +386,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
axis_index++;
|
||||
|
||||
char buf[50];
|
||||
sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
|
||||
sprintf(buf, "[cmd] Please rotate around %c", axislabels[axis_index]);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
tune_confirm();
|
||||
|
||||
|
|
@ -373,7 +401,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
|
||||
// if ((axis_left / 1000) == 0 && axis_left > 0) {
|
||||
// char buf[50];
|
||||
// sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
|
||||
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
// }
|
||||
|
||||
|
|
@ -410,7 +438,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
calibration_counter++;
|
||||
} else {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "[commander] mag cal canceled");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] mag cal canceled");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -446,27 +474,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
/* announce and set new offset */
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting X mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting Y mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting Z mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||
fprintf(stderr, "[commander] Setting X mag scale failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting X mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||
fprintf(stderr, "[commander] Setting Y mag scale failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting Y mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||
fprintf(stderr, "[commander] Setting Z mag scale failed!\n");
|
||||
fprintf(stderr, "[cmd] Setting Z mag scale failed!\n");
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
|
|
@ -489,7 +517,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] mag calibration done");
|
||||
|
||||
tune_confirm();
|
||||
sleep(2);
|
||||
|
|
@ -498,7 +526,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] mag calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
/* disable calibration mode */
|
||||
|
|
@ -549,7 +577,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
calibration_counter++;
|
||||
} else {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] gyro calibration aborted, retry");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -565,15 +593,15 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
|
||||
|
||||
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting X gyro offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Y gyro offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Z gyro offset failed!");
|
||||
}
|
||||
|
||||
/* set offsets to actual value */
|
||||
|
|
@ -599,7 +627,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
// char buf[50];
|
||||
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "[commander] gyro calibration done");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] gyro calibration done");
|
||||
|
||||
tune_confirm();
|
||||
sleep(2);
|
||||
|
|
@ -607,7 +635,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
sleep(2);
|
||||
/* third beep by cal end routine */
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] gyro calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
close(sub_sensor_combined);
|
||||
|
|
@ -617,7 +645,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
{
|
||||
/* announce change */
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] keep it level and still");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] keep it level and still");
|
||||
/* set to accel calibration mode */
|
||||
status->flag_preflight_accel_calibration = true;
|
||||
state_machine_publish(status_pub, status, mavlink_fd);
|
||||
|
|
@ -655,7 +683,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
calibration_counter++;
|
||||
} else {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] acceleration calibration aborted");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -674,27 +702,27 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
float scale = 9.80665f / total_len;
|
||||
|
||||
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!");
|
||||
}
|
||||
|
||||
fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
|
|
@ -717,9 +745,9 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
}
|
||||
|
||||
//char buf[50];
|
||||
//sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
|
||||
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
|
||||
//mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "[commander] accel calibration done");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] accel calibration done");
|
||||
|
||||
tune_confirm();
|
||||
sleep(2);
|
||||
|
|
@ -727,7 +755,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
sleep(2);
|
||||
/* third beep by cal end routine */
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] accel calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
/* exit accel calibration mode */
|
||||
|
|
@ -853,15 +881,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
|||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
|
||||
|
||||
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD starting gyro calibration");
|
||||
tune_confirm();
|
||||
do_gyro_calibration(status_pub, ¤t_status);
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD finished gyro calibration");
|
||||
tune_confirm();
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] REJECTING gyro calibration");
|
||||
result = MAV_RESULT_DENIED;
|
||||
}
|
||||
handled = true;
|
||||
|
|
@ -873,15 +901,50 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
|||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
|
||||
|
||||
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD starting mag calibration");
|
||||
tune_confirm();
|
||||
do_mag_calibration(status_pub, ¤t_status);
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD finished mag calibration");
|
||||
tune_confirm();
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration");
|
||||
result = MAV_RESULT_DENIED;
|
||||
}
|
||||
handled = true;
|
||||
}
|
||||
|
||||
/* zero-altitude pressure calibration */
|
||||
if ((int)(cmd->param3) == 1) {
|
||||
/* transition to calibration state */
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
|
||||
|
||||
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] zero altitude not implemented");
|
||||
tune_confirm();
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration");
|
||||
result = MAV_RESULT_DENIED;
|
||||
}
|
||||
handled = true;
|
||||
}
|
||||
|
||||
/* trim calibration */
|
||||
if ((int)(cmd->param4) == 1) {
|
||||
/* transition to calibration state */
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
|
||||
|
||||
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD starting mag calibration");
|
||||
tune_confirm();
|
||||
do_rc_calibration(status_pub, ¤t_status);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD finished mag calibration");
|
||||
tune_confirm();
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration");
|
||||
result = MAV_RESULT_DENIED;
|
||||
}
|
||||
handled = true;
|
||||
|
|
@ -893,15 +956,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
|||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
|
||||
|
||||
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD starting accel calibration");
|
||||
tune_confirm();
|
||||
do_accel_calibration(status_pub, ¤t_status);
|
||||
tune_confirm();
|
||||
mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] CMD finished accel calibration");
|
||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration");
|
||||
result = MAV_RESULT_DENIED;
|
||||
}
|
||||
handled = true;
|
||||
|
|
@ -909,8 +972,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
|||
|
||||
/* none found */
|
||||
if (!handled) {
|
||||
//fprintf(stderr, "[commander] refusing unsupported calibration request\n");
|
||||
mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request");
|
||||
//fprintf(stderr, "[cmd] refusing unsupported calibration request\n");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CMD refusing unsup. calib. request");
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
}
|
||||
|
|
@ -1172,7 +1235,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_t _param_sys_type = param_find("MAV_TYPE");
|
||||
|
||||
/* welcome user */
|
||||
printf("[commander] I am in command now!\n");
|
||||
printf("[cmd] I am in command now!\n");
|
||||
|
||||
/* pthreads for command and subsystem info handling */
|
||||
// pthread_t command_handling_thread;
|
||||
|
|
@ -1180,17 +1243,17 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* initialize */
|
||||
if (led_init() != 0) {
|
||||
fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n");
|
||||
fprintf(stderr, "[cmd] ERROR: Failed to initialize leds\n");
|
||||
}
|
||||
|
||||
if (buzzer_init() != 0) {
|
||||
fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n");
|
||||
fprintf(stderr, "[cmd] ERROR: Failed to initialize buzzer\n");
|
||||
}
|
||||
|
||||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
if (mavlink_fd < 0) {
|
||||
fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
|
||||
fprintf(stderr, "[cmd] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
|
||||
}
|
||||
|
||||
/* make sure we are in preflight state */
|
||||
|
|
@ -1212,11 +1275,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
state_machine_publish(stat_pub, ¤t_status, mavlink_fd);
|
||||
|
||||
if (stat_pub < 0) {
|
||||
printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n");
|
||||
printf("[cmd] ERROR: orb_advertise for topic vehicle_status failed.\n");
|
||||
exit(ERROR);
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[commander] system is running");
|
||||
mavlink_log_info(mavlink_fd, "[cmd] system is running");
|
||||
|
||||
/* create pthreads */
|
||||
pthread_attr_t subsystem_info_attr;
|
||||
|
|
@ -1344,7 +1407,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
} else {
|
||||
current_status.flag_external_manual_override_ok = true;
|
||||
}
|
||||
printf("param changed, val: %d, override: %s\n", current_status.system_type, (current_status.flag_external_manual_override_ok) ? "ON" : "OFF");
|
||||
} else {
|
||||
printf("ARMED, rejecting sys type change\n");
|
||||
}
|
||||
|
|
@ -1453,7 +1515,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
|
||||
low_battery_voltage_actions_done = true;
|
||||
mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
|
||||
}
|
||||
|
||||
low_voltage_counter++;
|
||||
|
|
@ -1463,7 +1525,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
|
||||
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
|
||||
critical_battery_voltage_actions_done = true;
|
||||
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
|
||||
state_machine_emergency(stat_pub, ¤t_status, mavlink_fd);
|
||||
}
|
||||
|
||||
|
|
@ -1591,8 +1653,16 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
|
||||
|
||||
/* check if left stick is in lower left position --> switch to standby state */
|
||||
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
|
||||
/*
|
||||
* Check if left stick is in lower left position --> switch to standby state.
|
||||
* Do this only for multirotors, not for fixed wing aircraft.
|
||||
*/
|
||||
if (((current_status.system_type == MAV_TYPE_QUADROTOR) ||
|
||||
(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
|
||||
(current_status.system_type == MAV_TYPE_OCTOROTOR)
|
||||
) &&
|
||||
((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
|
||||
(sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd);
|
||||
stick_on_counter = 0;
|
||||
|
|
@ -1604,7 +1674,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* check if left stick is in lower right position --> arm */
|
||||
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values
|
||||
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd);
|
||||
stick_on_counter = 0;
|
||||
|
|
@ -1614,17 +1684,16 @@ int commander_thread_main(int argc, char *argv[])
|
|||
stick_off_counter = 0;
|
||||
}
|
||||
}
|
||||
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
|
||||
|
||||
if (sp_man.aux1_cam_pan_flaps > STICK_ON_OFF_LIMIT) {
|
||||
if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
|
||||
update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd);
|
||||
} else {
|
||||
if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
//update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd);
|
||||
// XXX hack
|
||||
update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd);
|
||||
|
||||
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
|
||||
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
|
||||
update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd);
|
||||
|
||||
} else {
|
||||
|
|
@ -1635,9 +1704,9 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* handle the case where RC signal was regained */
|
||||
if (!current_status.rc_signal_found_once) {
|
||||
current_status.rc_signal_found_once = true;
|
||||
mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME.");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] DETECTED RC SIGNAL FIRST TIME.");
|
||||
} else {
|
||||
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
|
||||
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
|
||||
}
|
||||
|
||||
current_status.rc_signal_cutting_off = false;
|
||||
|
|
@ -1650,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
|
||||
/* only complain if the offboard control is NOT active */
|
||||
current_status.rc_signal_cutting_off = true;
|
||||
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL - NO REMOTE SIGNAL!");
|
||||
last_print_time = hrt_absolute_time();
|
||||
}
|
||||
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
|
||||
|
|
@ -1709,10 +1778,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
state_changed = true;
|
||||
tune_confirm();
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] DETECTED OFFBOARD CONTROL SIGNAL FIRST");
|
||||
} else {
|
||||
if (current_status.offboard_control_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] OK:RECOVERY OFFBOARD CONTROL");
|
||||
state_changed = true;
|
||||
tune_confirm();
|
||||
}
|
||||
|
|
@ -1736,7 +1805,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
|
||||
if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
|
||||
current_status.offboard_control_signal_weak = true;
|
||||
mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!");
|
||||
mavlink_log_critical(mavlink_fd, "[cmd] CRIT:NO OFFBOARD CONTROL!");
|
||||
last_print_time = hrt_absolute_time();
|
||||
}
|
||||
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
|
||||
|
|
@ -1799,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
close(sensor_sub);
|
||||
close(cmd_sub);
|
||||
|
||||
printf("[commander] exiting..\n");
|
||||
printf("[cmd] exiting..\n");
|
||||
fflush(stdout);
|
||||
|
||||
thread_running = false;
|
||||
|
|
|
|||
Loading…
Reference in New Issue