diff options
45 files changed, 2220 insertions, 1382 deletions
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 4152494e0..69d791da5 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ] then echo "[init] reading /fs/microsd/etc/rc" sh /fs/microsd/etc/rc -else - echo "[init] script /fs/microsd/etc/rc not present" +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt fi # diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4fc2e0452..6879f9dc8 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -65,9 +65,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); /* magnetometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 17087ab8a..f7e93db6c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -59,7 +59,6 @@ #include <errno.h> #include <debug.h> #include <sys/prctl.h> -#include <v1.0/common/mavlink.h> #include <string.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -73,10 +72,12 @@ #include <uORB/topics/battery_status.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> @@ -94,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; @@ -143,7 +147,6 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); -static void tune_confirm(void); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -151,6 +154,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); @@ -178,7 +182,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; } @@ -196,12 +200,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; } @@ -267,6 +271,34 @@ void tune_confirm(void) { ioctl(buzzer, TONE_SET_ALARM, 3); } +void tune_error(void) { + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +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) { @@ -309,7 +341,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 */ @@ -357,7 +389,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(); @@ -372,7 +404,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); // } @@ -409,7 +441,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; } } @@ -445,27 +477,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 */ @@ -488,7 +520,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); @@ -497,7 +529,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 */ @@ -548,7 +580,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; } } @@ -564,15 +596,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 */ @@ -598,7 +630,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); @@ -606,7 +638,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); @@ -616,7 +648,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); @@ -654,7 +686,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; } } @@ -673,27 +705,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); @@ -716,9 +748,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); @@ -726,7 +758,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 */ @@ -741,7 +773,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ - uint8_t result = MAV_RESULT_UNSUPPORTED; + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command handling */ tune_confirm(); @@ -751,99 +783,74 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { - case MAV_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } break; - case MAV_CMD_COMPONENT_ARM_DISARM: { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; /* request for an autopilot reboot */ - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* system may return here */ - result = MAV_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_DENIED; } } } break; - case PX4_CMD_CONTROLLER_SELECTION: { - bool changed = false; - if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) { - current_vehicle_status->flag_control_rates_enabled = cmd->param1; - changed = true; - } - if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) { - current_vehicle_status->flag_control_attitude_enabled = cmd->param2; - changed = true; - } - if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) { - current_vehicle_status->flag_control_velocity_enabled = cmd->param3; - changed = true; - } - if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) { - current_vehicle_status->flag_control_position_enabled = cmd->param4; - changed = true; - } - - if (changed) { - /* publish current state machine */ - state_machine_publish(status_pub, current_vehicle_status, mavlink_fd); - } - } - // /* request to land */ -// case MAV_CMD_NAV_LAND: +// case VEHICLE_CMD_NAV_LAND: // { // //TODO: add check if landing possible // //TODO: add landing maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } } // break; // // /* request to takeoff */ -// case MAV_CMD_NAV_TAKEOFF: +// case VEHICLE_CMD_NAV_TAKEOFF: // { // //TODO: add check if takeoff possible // //TODO: add takeoff maneuver // // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = MAV_RESULT_ACCEPTED; +// result = VEHICLE_CMD_RESULT_ACCEPTED; // } // } // break; // /* preflight calibration */ - case MAV_CMD_PREFLIGHT_CALIBRATION: { + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { bool handled = false; /* gyro calibration */ @@ -852,16 +859,16 @@ 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] 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] finished gyro calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING gyro calibration"); + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -872,16 +879,51 @@ 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] 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] finished mag calibration"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING mag calibration"); + result = VEHICLE_CMD_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 cal. not implemented"); + tune_confirm(); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING altitude calibration"); + result = VEHICLE_CMD_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] starting trim calibration"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[cmd] finished trim calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING trim calibration"); + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } @@ -892,32 +934,37 @@ 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; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration"); - result = MAV_RESULT_DENIED; + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration"); + result = VEHICLE_CMD_RESULT_DENIED; } handled = true; } /* none found */ if (!handled) { - //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); - result = MAV_RESULT_UNSUPPORTED; + //fprintf(stderr, "[cmd] refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "[cmd] CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; - case MAV_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed) { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed"); + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "[cmd] REJECTING save cmd while multicopter armed"); } else { // XXX move this to LOW PRIO THREAD of commander app @@ -931,11 +978,11 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); mavlink_log_info(mavlink_fd, "[cmd] OK loading params from"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (read_ret == 1) { mavlink_log_info(mavlink_fd, "[cmd] OK no changes in"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { if (read_ret < -1) { mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from"); @@ -944,7 +991,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + result = VEHICLE_CMD_RESULT_FAILED; } } else if (((int)(cmd->param1)) == 1) { @@ -954,7 +1001,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (write_ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] OK saved param file"); mavlink_log_info(mavlink_fd, param_get_default_file()); - result = MAV_RESULT_ACCEPTED; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { if (write_ret < -1) { @@ -964,12 +1011,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to"); mavlink_log_info(mavlink_fd, param_get_default_file()); } - result = MAV_RESULT_FAILED; + result = VEHICLE_CMD_RESULT_FAILED; } } else { mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } } @@ -977,7 +1024,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = MAV_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* announce command rejection */ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -985,11 +1032,11 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* supported command handling stop */ - if (result == MAV_RESULT_FAILED || - result == MAV_RESULT_DENIED || - result == MAV_RESULT_UNSUPPORTED) { + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { ioctl(buzzer, TONE_SET_ALARM, 5); - } else if (result == MAV_RESULT_ACCEPTED) { + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_confirm(); } @@ -1168,8 +1215,10 @@ int commander_thread_main(int argc, char *argv[]) failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + 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; @@ -1177,17 +1226,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 */ @@ -1200,6 +1249,10 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1207,11 +1260,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; @@ -1250,9 +1303,15 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; @@ -1263,10 +1322,16 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + /* subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1280,6 +1345,8 @@ int commander_thread_main(int argc, char *argv[]) uint64_t failsave_ll_start_time = 0; bool state_changed = true; + bool param_init_forced = true; + while (!thread_should_exit) { @@ -1294,8 +1361,13 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); if (new_data) { @@ -1305,6 +1377,46 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(stat_pub, ¤t_status, &cmd); } + /* update parameters */ + orb_check(param_changed_sub, &new_data); + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + } else { + current_status.flag_external_manual_override_ok = true; + } + } else { + printf("ARMED, rejecting sys type change\n"); + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } orb_check(battery_sub, &new_data); if (new_data) { @@ -1386,14 +1498,18 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS 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++; @@ -1403,7 +1519,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); } @@ -1416,6 +1532,76 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + /* Check if last transition deserved an audio event */ // #warning This code depends on state that is no longer? maintained // #if 0 @@ -1481,8 +1667,97 @@ 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 manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // printf("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } + + // printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * 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 == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_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; @@ -1494,7 +1769,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; @@ -1504,24 +1779,32 @@ 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.override_mode_switch > STICK_ON_OFF_LIMIT) { + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { + /* check auto mode switch for correct mode */ + if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + /* enable guided mode */ + update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + } } else { + /* center stick position, set SAS for all vehicle types */ update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } /* 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; @@ -1534,7 +1817,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) */ @@ -1593,10 +1876,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(); } @@ -1620,7 +1903,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) */ @@ -1679,11 +1962,11 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); - close(gps_sub); + close(global_position_sub); close(sensor_sub); close(cmd_sub); - printf("[commander] exiting..\n"); + printf("[cmd] exiting..\n"); fflush(stdout); thread_running = false; diff --git a/apps/commander/commander.h b/apps/commander/commander.h index bea67bead..04b4e72ab 100644 --- a/apps/commander/commander.h +++ b/apps/commander/commander.h @@ -52,4 +52,7 @@ #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f +void tune_confirm(void); +void tune_error(void); + #endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 46793c89b..18337a4ee 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); + fprintf(stderr, "[cmd] EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: @@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); + fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: @@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* prevent actuators from arming */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); + fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system"); break; case SYSTEM_STATE_PREFLIGHT: @@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state"); } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state"); } break; @@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); + mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } break; @@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* standby enforces disarmed */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: @@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: @@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: @@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: @@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode"); break; default: @@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con ret = OK; } if (invalid_state) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition"); ret = ERROR; } return ret; @@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } void publish_armed_status(const struct vehicle_status_s *current_status) { @@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { */ void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); + fprintf(stderr, "[cmd] EMERGENCY HANDLER\n"); /* Depending on the current state go to one of the error states */ if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { @@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); } else { - fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine); } } @@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); } else { - //global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); } } @@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[commander] arming\n"); + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } } @@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[commander] going standby\n"); + printf("[cmd] going standby\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] MISSION ABORT!\n"); + printf("[cmd] MISSION ABORT!\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } } @@ -518,53 +518,128 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->flag_control_manual_enabled = true; - /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = false; + } + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] manual mode\n"); + printf("[cmd] manual mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); } } void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] att stabilized mode\n"); + int old_mode = current_status->flight_mode; + int old_manual_control_mode = current_status->manual_control_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + current_status->flag_control_manual_enabled = true; + if (old_mode != current_status->flight_mode || + old_manual_control_mode != current_status->manual_control_mode) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + state_machine_publish(status_pub, current_status, mavlink_fd); + } + + } +} +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); + tune_error(); + return; + } if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] stabilized mode\n"); + printf("[cmd] position guided mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } } void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[commander] auto mode\n"); + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { - printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!") + } else { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { @@ -573,7 +648,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; - printf("[commander] arming due to command request\n"); + printf("[cmd] arming due to command request\n"); } } @@ -583,26 +658,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; - printf("[commander] disarming due to command request\n"); + printf("[cmd] disarming due to command request\n"); } } - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.") - } - /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL"); ret = ERROR; } @@ -627,7 +690,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { printf("system will reboot\n"); - //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + mavlink_log_critical(mavlink_fd, "[cmd] Rebooting.."); + usleep(200000); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); ret = 0; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index c4d1b78a5..2f2ccc729 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -128,6 +128,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); /** + * Handle state machine if mode switch is guided + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** * Handle state machine if mode switch is auto * * @param status_pub file descriptor for state update topic publication diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index c2aed9b54..2a7bfe3d7 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -148,9 +148,7 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER hrt_init(); -#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -158,27 +156,21 @@ __EXPORT int nsh_archinitialize(void) #endif /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); // initial LED state drv_led_start(); @@ -233,7 +225,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN10); stm32_configgpio(GPIO_ADC1_IN11); - //stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available? + stm32_configgpio(GPIO_ADC1_IN12); //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards return OK; diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 78c061819..fe9b281f6 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -// #include <drivers/boards/HIL/HIL_internal.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_mixer.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } @@ -396,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ @@ -419,9 +429,6 @@ HIL::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status */ - // up_pwm_servo_arm(aa.armed); } } diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 3b835904f..430d18c6d 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,6 +58,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> #include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> @@ -65,6 +66,7 @@ #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> @@ -97,6 +99,7 @@ private: int _t_actuators; int _t_armed; orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,6 +165,7 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_armed(-1), _t_outputs(0), + _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -319,6 +323,13 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -336,8 +347,16 @@ PX4FMU::task_main() if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); @@ -364,20 +383,39 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } } @@ -394,6 +432,7 @@ PX4FMU::task_main() } ::close(_t_actuators); + ::close(_t_actuators_effective); ::close(_t_armed); /* make sure servos are off */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 82ff61808..332d4f882 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -55,6 +55,7 @@ #include <unistd.h> #include <termios.h> #include <fcntl.h> +#include <math.h> #include <arch/board/board.h> @@ -70,9 +71,13 @@ #include <systemlib/hx_stream.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> +#include <systemlib/scheduling_priorities.h> +#include <systemlib/param/param.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> @@ -90,10 +95,18 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + /** + * Set the PWM via serial update rate + * @warning this directly affects CPU load + */ + int set_pwm_rate(int hz); + bool dump_one; private: + // XXX static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; + unsigned _update_rate; ///< serial send rate in Hz int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -105,8 +118,13 @@ private: int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + int _t_vstatus; ///< system / vehicle status + vehicle_status_s _vstatus; ///< overall system state orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values @@ -191,13 +209,16 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), dump_one(false), + _update_rate(50), _serial_fd(-1), _io_stream(nullptr), _task(-1), _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), + _t_vstatus(-1), _t_outputs(-1), _mix_buf(nullptr), _mix_buf_len(0), @@ -205,7 +226,7 @@ PX4IO::PX4IO() : _relays(0), _switch_armed(false), _send_needed(false), - _config_needed(false) + _config_needed(true) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -253,7 +274,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -279,6 +300,17 @@ PX4IO::init() return OK; } +int +PX4IO::set_pwm_rate(int hz) +{ + if (hz > 0 && hz <= 400) { + _update_rate = hz; + return OK; + } else { + return -EINVAL; + } +} + void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -288,7 +320,8 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - debug("starting"); + log("starting"); + unsigned update_rate_in_ms; /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); @@ -328,12 +361,20 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); /* convert the update rate in hz to milliseconds, rounding down if necessary */ - //int update_rate_in_ms = int(1000 / _update_rate); - orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ + update_rate_in_ms = 1000 / _update_rate; + orb_set_interval(_t_actuators, update_rate_in_ms); _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -348,13 +389,15 @@ PX4IO::task_main() _to_battery = -1; /* poll descriptor */ - pollfd fds[3]; + pollfd fds[4]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; fds[1].fd = _t_actuators; fds[1].events = POLLIN; fds[2].fd = _t_armed; fds[2].events = POLLIN; + fds[3].fd = _t_vstatus; + fds[3].events = POLLIN; debug("ready"); @@ -388,7 +431,8 @@ PX4IO::task_main() if (fds[1].revents & POLLIN) { /* get controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* scale controls to PWM (temporary measure) */ for (unsigned i = 0; i < _max_actuators; i++) @@ -404,12 +448,11 @@ PX4IO::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } - } - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); + _send_needed = true; + } } /* send a config packet to IO if required */ @@ -426,6 +469,12 @@ PX4IO::task_main() _mix_buf = nullptr; _mix_buf_len = 0; } + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } } unlock(); @@ -564,20 +613,27 @@ PX4IO::io_send() cmd.f2i_magic = F2I_MAGIC; /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_actuators; i++) { cmd.output_control[i] = _outputs.output[i]; - + } /* publish as we send */ + _outputs.timestamp = hrt_absolute_time(); /* XXX needs to be based off post-mix values from the IO side */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); /* update relays */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - /* armed and not locked down */ + /* armed and not locked down -> arming ok */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); - + /* indicate that full autonomous position control / vector flight mode is available */ + cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; + /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ + cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; + /* set desired PWM output rate */ + cmd.servo_rate = _update_rate; + ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); if (ret) @@ -592,6 +648,46 @@ PX4IO::config_send() cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + int val; + + /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ + param_get(param_find("RC_MAP_ROLL"), &val); + cfg.rc_map[0] = (uint8_t)val; + param_get(param_find("RC_MAP_PITCH"), &val); + cfg.rc_map[1] = (uint8_t)val; + param_get(param_find("RC_MAP_YAW"), &val); + cfg.rc_map[2] = (uint8_t)val; + param_get(param_find("RC_MAP_THROTTLE"), &val); + cfg.rc_map[3] = (uint8_t)val; + + /* set the individual channel properties */ + char nbuf[16]; + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MIN", i); + param_get(param_find(nbuf), &val); + cfg.rc_min[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_TRIM", i); + param_get(param_find(nbuf), &val); + cfg.rc_trim[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MAX", i); + param_get(param_find(nbuf), &val); + cfg.rc_max[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_REV", i); + param_get(param_find(nbuf), &val); + cfg.rc_rev[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_DZ", i); + param_get(param_find(nbuf), &val); + cfg.rc_dz[i] = (uint16_t)val; + } + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); if (ret) @@ -778,7 +874,7 @@ test(void) void monitor(void) { - unsigned cancels = 4; + unsigned cancels = 3; printf("Hit <enter> three times to exit monitor mode\n"); for (;;) { @@ -800,6 +896,7 @@ monitor(void) g_dev->dump_one = true; } } + } int @@ -821,6 +918,16 @@ px4io_main(int argc, char *argv[]) errx(1, "driver init failed"); } + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 + 1) { + g_dev->set_pwm_rate(atoi(argv[2 + 1])); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + } + exit(0); } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 334b95226..957036b41 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -113,6 +113,7 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp) { static int counter = 0; @@ -145,12 +146,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; - rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); + + /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ + float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); + /* set pitch plus feedforward roll compensation */ + rates_sp->pitch = pid_calculate(&pitch_controller, + att_sp->pitch_body + pitch_sp_rollcompensation, + att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ - //TODO + rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) + / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); + +// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/apps/fixedwing_att_control/fixedwing_att_control_att.h index ca7c14b43..11c932800 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.h +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.h @@ -41,9 +41,11 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_global_position.h> int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp); #endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 38f58ac33..a36f1ce7d 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -58,40 +58,16 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> - #include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_att.h> -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - /* Prototypes */ /** * Deamon management function. @@ -126,7 +102,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing att control] started\n"); /* declare and safely initialize all structs */ struct vehicle_attitude_s att; @@ -135,14 +111,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&att_sp, 0, sizeof(att_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct manual_control_setpoint_s manual_sp; memset(&manual_sp, 0, sizeof(manual_sp)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); -// static struct debug_key_value_s debug_output; -// memset(&debug_output, 0, sizeof(debug_output)); - /* output structs */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -153,18 +128,18 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); -// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); -// debug_output.key[0] = '1'; - + orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); /* subscribe */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; + float speed_body[3] = {0.0f, 0.0f, 0.0f}; struct pollfd fds = { .fd = att_sub, .events = POLLIN }; while(!thread_should_exit) @@ -172,9 +147,35 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* wait for a sensor update, check for exit condition every 500 ms */ poll(&fds, 1, 500); + /* Check if there is a new position measurement or attitude setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool att_sp_updated; + orb_check(att_sp_sub, &att_sp_updated); + /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + if(att_sp_updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + if(pos_updated) + { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + if(att.R_valid) + { + speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; + speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; + speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; + } + else + { + speed_body[0] = 0; + speed_body[1] = 0; + speed_body[2] = 0; + + printf("FW ATT CONTROL: Did not get a valid R\n"); + } + } + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); @@ -182,33 +183,96 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* Control */ + /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO) { - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &rates_sp); + if (vstatus.state_machine == SYSTEM_STATE_AUTO || + vstatus.state_machine == SYSTEM_STATE_STABILIZED) { + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - /* Attitude Rate Control */ + /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - //REMOVEME XXX - actuators.control[3] = 0.7f; + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; - // debug_output.value = rates_sp.pitch; - // orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = -manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; + if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (vstatus.rc_signal_lost) { + + /* put plane into loiter */ + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + + /* limit throttle to 60 % of last value if sane */ + if (isfinite(manual_sp.throttle) && + (manual_sp.throttle >= 0.0f) && + (manual_sp.throttle <= 1.0f)) { + att_sp.thrust = 0.6f * manual_sp.throttle; + } else { + att_sp.thrust = 0.0f; + } + att_sp.yaw_body = 0; + + // XXX disable yaw control, loiter + + } else { + + att_sp.roll_body = manual_sp.roll; + att_sp.pitch_body = manual_sp.pitch; + att_sp.yaw_body = 0; + att_sp.thrust = manual_sp.throttle; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* pass through flaps */ + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + } else { + actuators.control[4] = 0.0f; + } + + } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + /* directly pass through values */ + actuators.control[0] = manual_sp.roll; + /* positive pitch means negative actuator -> pull up */ + actuators.control[1] = manual_sp.pitch; + actuators.control[2] = manual_sp.yaw; + actuators.control[3] = manual_sp.throttle; + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + } else { + actuators.control[4] = 0.0f; + } + } } - /* publish output */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* publish rates */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + + /* sanity check and publish actuator outputs */ + if (isfinite(actuators.control[0]) && + isfinite(actuators.control[1]) && + isfinite(actuators.control[2]) && + isfinite(actuators.control[3])) + { + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } } printf("[fixedwing_att_control] exiting, stopping all motors.\n"); @@ -224,6 +288,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) close(att_sub); close(actuator_pub); + close(rates_pub); fflush(stdout); exit(0); @@ -268,7 +333,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_att_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index b81d50168..f3e36c0ec 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,8 @@ /** * @file fixedwing_att_control_rate.c * Implementation of a fixed wing attitude controller. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include <fixedwing_att_control_rate.h> @@ -59,9 +61,33 @@ #include <systemlib/geo/geo.h> #include <systemlib/systemlib.h> - - - +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* feedforward compensation */ +PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ struct fw_rate_control_params { float rollrate_p; @@ -73,7 +99,7 @@ struct fw_rate_control_params { float yawrate_p; float yawrate_i; float yawrate_awu; - + float pitch_thr_ff; }; struct fw_rate_control_param_handles { @@ -86,7 +112,7 @@ struct fw_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_awu; - + param_t pitch_thr_ff; }; @@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); return OK; } @@ -124,7 +151,7 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_awu, &(p->yawrate_awu)); - + param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); return OK; } @@ -167,12 +194,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } - /* Roll Rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); - - - actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... - actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now + /* roll rate (PI) */ + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + /* pitch rate (PI) */ + actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ + actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; + /* yaw rate (PI) */ + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); counter++; diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile deleted file mode 100644 index 02d4463dd..000000000 --- a/apps/fixedwing_control/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# fixedwing_control Application -# - -APPNAME = fixedwing_control -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -CSRCS = fixedwing_control.c - -include $(APPDIR)/mk/app.mk diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c deleted file mode 100644 index e04033481..000000000 --- a/apps/fixedwing_control/fixedwing_control.c +++ /dev/null @@ -1,566 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov <oivan@ethz.ch> - * Modifications: Doug Weibel <douglas.weibel@colorado.edu> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fixedwing_control.c - * Implementation of a fixed wing attitude and position controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> -#include <uORB/topics/debug_key_value.h> - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - -struct fw_att_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float rollrate_lim; - float roll_p; - float roll_lim; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float pitchrate_lim; - float pitch_p; - float pitch_lim; -}; - -struct fw_att_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t rollrate_lim; - param_t roll_p; - param_t roll_lim; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t pitchrate_lim; - param_t pitch_p; - param_t pitch_lim; -}; - - -// TO_DO - Navigation control will be moved to a separate app -// Attitude control will just handle the inner angle and rate loops -// to control pitch and roll, and turn coordination via rudder and -// possibly throttle compensation for battery voltage sag. - -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); - -struct fw_pos_control_params { - float heading_p; - float heading_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t heading_lim; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int att_parameters_init(struct fw_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); - -/** - * Initialize all parameter handles and values - * - */ -static int pos_parameters_init(struct fw_pos_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - - -/** - * The fixed wing control main thread. - * - * The main loop executes continously and calculates the control - * response. - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -int fixedwing_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct vehicle_attitude_s att; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* subscribe to attitude, motor setpoints and system state */ - struct vehicle_global_position_s global_pos; - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_setpoint_s global_setpoint; - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - - uint64_t last_run = 0; - uint64_t last_run_pos = 0; - - bool global_sp_updated_set_once = false; - - struct fw_att_control_params p; - struct fw_att_control_param_handles h; - - struct fw_pos_control_params ppos; - struct fw_pos_control_param_handles hpos; - - /* initialize the pid controllers */ - att_parameters_init(&h); - att_parameters_update(&h, &p); - - pos_parameters_init(&hpos); - pos_parameters_update(&hpos, &ppos); - -// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere - PID_t roll_rate_controller; - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, - p.rollrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t roll_angle_controller; - pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, - p.roll_lim,PID_MODE_DERIVATIV_NONE); - - PID_t pitch_rate_controller; - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, - p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t pitch_angle_controller; - pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, - p.pitch_lim,PID_MODE_DERIVATIV_NONE); - - PID_t heading_controller; - pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit - - // XXX remove in production - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - // This is the top of the main loop - while(!thread_should_exit) { - - struct pollfd fds[1] = { - { .fd = att_sub, .events = POLLIN }, - }; - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); - } else { - - // FIXME SUBSCRIBE - if (loopcounter % 100 == 0) { - att_parameters_update(&h, &p); - pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, - p.rollrate_awu, p.rollrate_lim); - pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, - 0.0f, p.roll_lim); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, - p.pitchrate_awu, p.pitchrate_lim); - pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, - 0.0f, p.pitch_lim); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); -//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", -//p.rollrate_p, p.rollrate_i, p.rollrate_lim); - } - - /* if position updated, run position control loop */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - if (global_sp_updated) { - global_sp_updated_set_once = true; - } - /* checking has to happen before the read, as the read clears the changed flag */ - - /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - //orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - // XXX update to switch between external attitude reference and the - // attitude calculated here - - char name[10]; - - if (pos_updated) { - - /* get position */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (global_sp_updated_set_once) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - - /* calculate delta T */ - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* calculate bearing error */ - float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, - global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); - - /* shift error to prevent wrapping issues */ - float bearing_error = target_bearing - att.yaw; - - if (loopcounter % 2 == 0) { - sprintf(name, "hdng err1"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - if (bearing_error < M_PI_F) { - bearing_error += 2.0f * M_PI_F; - } - - if (bearing_error > M_PI_F) { - bearing_error -= 2.0f * M_PI_F; - } - - if (loopcounter % 2 != 0) { - sprintf(name, "hdng err2"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - /* calculate roll setpoint, do this artificially around zero */ - att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, - 0.0f, att.yawspeed, deltaT); - - /* limit roll angle output */ - if (att_sp.roll_body > ppos.heading_lim) { - att_sp.roll_body = ppos.heading_lim; - heading_controller.saturated = 1; - } - - if (att_sp.roll_body < -ppos.heading_lim) { - att_sp.roll_body = -ppos.heading_lim; - heading_controller.saturated = 1; - } - - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - - } else { - /* no setpoint, maintain level flight */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - } - - att_sp.thrust = 0.7f; - } - - /* calculate delta T */ - const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; - last_run_pos = hrt_absolute_time(); - - if (verbose && (loopcounter % 20 == 0)) { - printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); - } - - // actuator control[0] is aileron (or elevon roll control) - // Commanded roll rate from P controller on roll angle - float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, - att.roll, 0.0f, deltaTpos); - // actuator control from PI controller on roll rate - actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, - att.rollspeed, 0.0f, deltaTpos); - - // actuator control[1] is elevator (or elevon pitch control) - // Commanded pitch rate from P controller on pitch angle - float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, - att.pitch, 0.0f, deltaTpos); - // actuator control from PI controller on pitch rate - actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, - att.pitchspeed, 0.0f, deltaTpos); - - // actuator control[3] is throttle - actuators.control[3] = att_sp.thrust; - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - } - } - - printf("[fixedwing_control] exiting.\n"); - thread_running = false; - - return 0; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 4096, - fixedwing_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_control is running\n"); - } else { - printf("\tfixedwing_control not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int att_parameters_init(struct fw_att_control_param_handles *h) -{ - /* PID parameters */ - - h->rollrate_p = param_find("FW_ROLLRATE_P"); - h->rollrate_i = param_find("FW_ROLLRATE_I"); - h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); - h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); - h->roll_p = param_find("FW_ROLL_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitchrate_p = param_find("FW_PITCHRATE_P"); - h->pitchrate_i = param_find("FW_PITCHRATE_I"); - h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); - h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->roll_p, &(p->roll_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - -static int pos_parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->heading_lim = param_find("FW_HEADING_LIM"); - - return OK; -} - -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->heading_lim, &(p->heading_lim)); - - return OK; -} diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index a70b9bd30..fbd6138de 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -57,24 +57,31 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> /* * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - +PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ struct fw_pos_control_params { float heading_p; + float headingr_p; + float headingr_i; + float headingr_lim; float xtrack_p; float altitude_p; float roll_lim; @@ -83,11 +90,13 @@ struct fw_pos_control_params { struct fw_pos_control_param_handles { param_t heading_p; + param_t headingr_p; + param_t headingr_i; + param_t headingr_lim; param_t xtrack_p; param_t altitude_p; param_t roll_lim; param_t pitch_lim; - }; @@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - + h->heading_p = param_find("FW_HEAD_P"); + h->headingr_p = param_find("FW_HEADR_P"); + h->headingr_i = param_find("FW_HEADR_I"); + h->headingr_lim = param_find("FW_HEADR_LIM"); + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } @@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h) static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { param_get(h->heading_p, &(p->heading_p)); + param_get(h->headingr_p, &(p->headingr_p)); + param_get(h->headingr_i, &(p->headingr_i)); + param_get(h->headingr_lim, &(p->headingr_lim)); param_get(h->xtrack_p, &(p->xtrack_p)); param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); @@ -171,7 +185,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing pos control] started\n"); /* declare and safely initialize all structs */ struct vehicle_global_position_s global_pos; @@ -184,6 +198,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct crosstrack_error_s xtrack_err; memset(&xtrack_err, 0, sizeof(xtrack_err)); + struct parameter_update_s param_update; + memset(¶m_update, 0, sizeof(param_update)); /* output structs */ struct vehicle_attitude_setpoint_s attitude_setpoint; @@ -193,129 +209,196 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) attitude_setpoint.roll_body = 0.0f; attitude_setpoint.pitch_body = 0.0f; attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.thrust = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + struct pollfd fds[2] = { + { .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN } + }; bool global_sp_updated_set_once = false; float psi_track = 0.0f; - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - static int counter = 0; - static bool initialized = false; - - static struct fw_pos_control_params p; - static struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t altitude_controller; - - if(!initialized) - { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - - } - - /* Check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* Load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(pos_updated) - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - if(global_sp_updated) { - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", (double)psi_track); - } - - /* Control */ - - - /* Simple Horizontal Control */ - if(global_sp_updated_set_once) - { -// if (counter % 100 == 0) -// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - if(!(distance_res != OK || xtrack_err.past_end)) { + int counter = 0; - float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + struct fw_pos_control_params p; + struct fw_pos_control_param_handles h; - if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) - delta_psi_c = 60.0f*M_DEG_TO_RAD_F; + PID_t heading_controller; + PID_t heading_rate_controller; + PID_t offtrack_controller; + PID_t altitude_controller; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) - delta_psi_c = -60.0f*M_DEG_TO_RAD_F; + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value - float psi_c = psi_track + delta_psi_c; + /* error and performance monitoring */ + perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); + perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - float psi_e = psi_c - att.yaw; - - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); - - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - -// if (counter % 100 == 0) -// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { - if (counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + while(!thread_should_exit) + { + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(fw_err_perf); + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); + + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value } - } - - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + printf("next wp direction: %0.4f\n", (double)psi_track); + } + + /* Simple Horizontal Control */ + if(global_sp_updated_set_once) + { + // if (counter % 100 == 0) + // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + // XXX what is xtrack_err.past_end? + if(distance_res == OK /*&& !xtrack_err.past_end*/) { + + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + + float psi_c = psi_track + delta_psi_c; + float psi_e = psi_c - att.yaw; + + /* wrap difference back onto -pi..pi range */ + psi_e = _wrap_pi(psi_e); + + if (verbose) { + printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); + printf("delta_psi_c %.4f ", (double)delta_psi_c); + printf("psi_c %.4f ", (double)psi_c); + printf("att.yaw %.4f ", (double)att.yaw); + printf("psi_e %.4f ", (double)psi_e); + } + + /* calculate roll setpoint, do this artificially around zero */ + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following + float psi_rate_c = delta_psi_rate_c + psi_rate_track; + + /* limit turn rate */ + if(psi_rate_c > p.headingr_lim) { + psi_rate_c = p.headingr_lim; + } else if(psi_rate_c < -p.headingr_lim) { + psi_rate_c = -p.headingr_lim; + } + + float psi_rate_e = psi_rate_c - att.yawspeed; + + // XXX sanity check: Assume 10 m/s stall speed and no stall condition + float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + + if (ground_speed < 10.0f) { + ground_speed = 10.0f; + } + + float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g + + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + + if (verbose) { + printf("psi_rate_c %.4f ", (double)psi_rate_c); + printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); + printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); + } + + if (verbose && counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } else { + if (verbose && counter % 100 == 0) + printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + } + + /* Very simple Altitude Control */ + if(pos_updated) + { + + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + + } + + // XXX need speed control + attitude_setpoint.thrust = 0.7f; + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + + counter++; + + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - counter++; } printf("[fixedwing_pos_control] exiting.\n"); @@ -367,7 +450,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 81b907bcd..ccc9d6d7d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -189,12 +189,32 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode = 0; /* set mode flags independent of system state */ + + /* HIL */ + if (v_status.flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* manual input */ if (v_status.flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.flag_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + /* attitude or rate control */ + if (v_status.flag_control_attitude_enabled || + v_status.flag_control_rates_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + /* vector control */ + if (v_status.flag_control_velocity_enabled || + v_status.flag_control_position_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + + /* autonomous mode */ + if (v_status.state_machine == SYSTEM_STATE_AUTO) { + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ @@ -225,17 +245,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) case SYSTEM_STATE_MANUAL: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case SYSTEM_STATE_STABILIZED: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; break; case SYSTEM_STATE_AUTO: *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case SYSTEM_STATE_MISSION_ABORT: @@ -470,14 +487,15 @@ mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) void mavlink_update_system(void) { static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + initialized = true; } /* update system and component id */ @@ -745,6 +763,7 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = true; while (thread_running) { usleep(200000); + printf("."); } warnx("terminated."); exit(0); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed..58761e89c 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg) offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX; if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { ml_armed = false; @@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if (mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -305,6 +325,7 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; + /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e6ec77bf6..40e52a500 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -455,7 +455,7 @@ l_actuator_outputs(struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -506,20 +506,19 @@ l_actuator_outputs(struct listener *l) mavlink_mode, 0); } else { - float rudder, throttle; - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; } mavlink_msg_hil_controls_send(chan, @@ -566,28 +565,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_controls_effective_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); + "eff ctrl0 ", + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); + "eff ctrl1 ", + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); + "eff ctrl2 ", + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + "eff ctrl3 ", + actuators.control_effective[3]); } } @@ -739,7 +738,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ce1e52c1b..1b03e8c22 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -232,19 +232,9 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - /* decide wether we want rate or position input */ - } - else if (state.flag_control_manual_enabled) { + - /* manual inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + } else if (state.flag_control_manual_enabled) { if (state.flag_control_attitude_enabled) { @@ -258,12 +248,25 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(state.rc_signal_lost) { + if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - att_sp.thrust = failsafe_throttle; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; + } else { + att_sp.thrust = 0.0f; + } /* keep current yaw, do not attempt to go to north orientation, * since if the pilot regains RC control, he will be lost regarding @@ -285,41 +288,66 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } - /* only move setpoint if manual input is != 0 */ + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + control_yaw_position = true; } - control_yaw_position = true; } - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } - } - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d..b98736cee 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -197,7 +197,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,6 +206,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } + /* reset integral if on ground */ + if(att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 65d199fe5..adedea74c 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> @@ -139,8 +140,9 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) { report.rc_channel[i] = system_state.rc_channel_data[i]; + } report.channel_count = system_state.rc_channels; report.armed = system_state.armed; @@ -205,7 +207,19 @@ comms_handle_config(const void *buffer, size_t length) if (length != sizeof(*cfg)) return; - /* XXX */ + /* fetch the rc mappings */ + for (unsigned i = 0; i < 4; i++) { + system_state.rc_map[i] = cfg->rc_map[i]; + } + + /* fetch the rc channel attributes */ + for (unsigned i = 0; i < 4; i++) { + system_state.rc_min[i] = cfg->rc_min[i]; + system_state.rc_trim[i] = cfg->rc_trim[i]; + system_state.rc_max[i] = cfg->rc_max[i]; + system_state.rc_rev[i] = cfg->rc_rev[i]; + system_state.rc_dz[i] = cfg->rc_dz[i]; + } } static void @@ -227,12 +241,34 @@ comms_handle_command(const void *buffer, size_t length) system_state.armed = false; system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; - system_state.fmu_data_received = true; + system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; + system_state.manual_override_ok = cmd->manual_override_ok; + system_state.mixer_fmu_available = true; + system_state.fmu_data_received_time = hrt_absolute_time(); + + /* set PWM update rate if changed (after limiting) */ + uint16_t new_servo_rate = cmd->servo_rate; + + /* reject faster than 500 Hz updates */ + if (new_servo_rate > 500) { + new_servo_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (new_servo_rate < 50) { + new_servo_rate = 50; + } + if (system_state.servo_rate != new_servo_rate) { + up_pwm_servo_set_rate(new_servo_rate); + system_state.servo_rate = new_servo_rate; + } - /* handle changes signalled by FMU */ -// if (!system_state.arm_ok && system_state.armed) -// system_state.armed = false; + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ + mixer_tick(); /* handle relay state changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 43e811ab0..da445e3b4 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -60,6 +60,10 @@ #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 + static void ppm_input(void); void @@ -88,6 +92,12 @@ controls_main(void) */ bool locked = false; + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + if (fds[0].revents & POLLIN) locked |= dsm_input(); @@ -107,22 +117,38 @@ controls_main(void) if (!locked) ppm_input(); + /* check for manual override status */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + /* force manual input override */ + system_state.mixer_manual_override = true; + } else { + /* override not engaged, use FMU */ + system_state.mixer_manual_override = false; + } + /* * If we haven't seen any new control data in 200ms, assume we * have lost input and tell FMU. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + if (system_state.rc_channels > 0) { + /* + * If the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due = true; + } + /* set the number of channels to zero - no inputs */ system_state.rc_channels = 0; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; } - /* XXX do bypass mode, etc. here */ - - /* do PWM output updates */ + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ mixer_tick(); } } @@ -141,8 +167,9 @@ ppm_input(void) /* PPM data exists, copy it */ system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) + for (unsigned i = 0; i < ppm_decoded_channels; i++) { system_state.rc_channel_data[i] = ppm_buffer[i]; + } /* copy the timestamp and clear it */ system_state.rc_channels_timestamp = ppm_last_valid_decode; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index d21b3a898..95484bc9d 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -47,8 +47,14 @@ #include <errno.h> #include <unistd.h> #include <fcntl.h> +#include <sched.h> + +#include <debug.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> +#include <drivers/device/device.h> + #include <systemlib/mixer/mixer.h> extern "C" { @@ -57,10 +63,15 @@ extern "C" { } /* - * Count of periodic calls in which we have no FMU input. + * Maximum interval in us before FMU signal is considered lost */ -static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 20 +#define FMU_INPUT_DROP_LIMIT_US 200000 + +/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ +#define ROLL 0 +#define PITCH 1 +#define YAW 2 +#define THROTTLE 3 /* current servo arm/disarm state */ bool mixer_servos_armed = false; @@ -69,6 +80,8 @@ bool mixer_servos_armed = false; static uint16_t *control_values; static int control_count; +static uint16_t rc_channel_data[PX4IO_CONTROL_CHANNELS]; + static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, @@ -81,37 +94,68 @@ mixer_tick(void) { bool should_arm; + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("RX timeout\n"); + } + /* * Decide which set of inputs we're using. */ - if (system_state.mixer_use_fmu) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - /* check that we are receiving fresh data from the FMU */ - if (!system_state.fmu_data_received) { - fmu_input_drops++; - - /* too many frames without FMU input, time to go to failsafe */ - if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_use_fmu = false; - } - + + /* this is for planes, where manual override makes sense */ + if (system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + + } else if (system_state.rc_channels > 0) { + /* when override is on or the fmu is not available, but RC is present */ + control_count = system_state.rc_channels; + + sched_lock(); + + /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ + rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; + rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; + rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; + rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; + + /* get the remaining channels, no remapping needed */ + for (unsigned i = 4; i < system_state.rc_channels; i++) + rc_channel_data[i] = system_state.rc_channel_data[i]; + + /* scale the control inputs */ + rc_channel_data[THROTTLE] = ((rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / + (system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000 + 1000; + //lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", + // system_state.rc_min[THROTTLE], system_state.rc_trim[THROTTLE], system_state.rc_max[THROTTLE], rc_channel_data[THROTTLE]); + + control_values = &rc_channel_data[0]; + sched_unlock(); } else { - fmu_input_drops = 0; - system_state.fmu_data_received = false; - } + /* we have no control input (no FMU, no RC) */ - } else if (system_state.rc_channels > 0) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + // XXX builtin failsafe would activate here + control_count = 0; + } + //lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input */ - /* XXX builtin failsafe would activate here */ - control_count = 0; + /* if the fmu is available whe are good */ + if (system_state.mixer_fmu_available) { + control_count = PX4IO_CONTROL_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* @@ -138,14 +182,17 @@ mixer_tick(void) /* * If we are armed, update the servo output. */ - if (system_state.armed && system_state.arm_ok) + if (system_state.armed && system_state.arm_ok) { up_pwm_servo_set(i, system_state.servos[i]); + } } } /* * Decide whether the servos should be armed right now. + * A sufficient reason is armed state and either FMU or RC control inputs */ + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { @@ -229,4 +276,4 @@ mixer_handle_text(const void *buffer, size_t length) break; } -}
\ No newline at end of file +} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index cb3ad6f2e..7aa3be02a 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -54,9 +54,12 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t output_control[PX4IO_CONTROL_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; + uint16_t servo_rate; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ + bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ + bool arm_ok; /**< FMU allows full arming */ + bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ + bool manual_override_ok; /**< if true, IO performs a direct manual override */ }; /** @@ -82,7 +85,12 @@ struct px4io_config { uint16_t f2i_config_magic; #define F2I_CONFIG_MAGIC 0x6366 - /* XXX currently nothing here */ + uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */ + uint16_t rc_min[4]; /**< min value for each channel */ + uint16_t rc_trim[4]; /**< trim value for each channel */ + uint16_t rc_max[4]; /**< max value for each channel */ + uint16_t rc_rev[4]; /**< rev value for each channel */ + uint16_t rc_dz[4]; /**< dz value for each channel */ }; /** diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 74362617d..bea9d59bc 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -66,6 +66,8 @@ int user_start(int argc, char *argv[]) /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); + /* default to 50 Hz PWM outputs */ + system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 46a6be4a8..65d33f34b 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -70,43 +70,89 @@ struct sys_state_s { bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ + uint16_t servo_rate; /* output rate of servos in Hz */ - /* + /** + * Remote control input(s) channel mappings + */ + uint8_t rc_map[4]; + + /** + * Remote control channel attributes + */ + uint16_t rc_min[4]; + uint16_t rc_trim[4]; + uint16_t rc_max[4]; + uint16_t rc_rev[4]; + uint16_t rc_dz[4]; + + /** * Data from the remote control input(s) */ unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; - /* + /** * Mixed servo outputs */ uint16_t servos[IO_SERVO_COUNT]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * If true, we are using the FMU controls, else RC input if available. */ - bool mixer_use_fmu; + bool mixer_manual_override; - /* + /** + * If true, FMU input is available. + */ + bool mixer_fmu_available; + + /** * If true, state that should be reported to FMU has been updated. */ bool fmu_report_due; - /* - * If true, new control data from the FMU has been received. + /** + * Last FMU receive time, in microseconds since system boot + */ + uint64_t fmu_data_received_time; + + /** + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * If true, the connection to FMU has been lost for more than a timeout interval + */ + bool fmu_lost; + + /** + * If true, FMU is ready for autonomous position control. Only used for LED indication + */ + bool vector_flight_mode_ok; + + /** + * If true, IO performs an on-board manual override with the RC channel values */ - bool fmu_data_received; + bool manual_override_ok; /* * Measured battery voltage in mV diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 93596ca75..3e02b717d 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,20 +58,27 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +static unsigned counter = 0; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0xffff // always on -#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking -#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking -#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink +#define LED_PATTERN_SAFE 0xffff /**< always on */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ +#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ +#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ static unsigned blink_counter = 0; +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 2 static bool safety_button_pressed; @@ -101,12 +108,16 @@ safety_check_button(void *arg) */ safety_button_pressed = BUTTON_SAFETY; - if (safety_button_pressed) { - //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); - } - - /* Keep pressed for a while to arm */ + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -120,10 +131,11 @@ safety_check_button(void *arg) /* Disarm quickly */ } else if (safety_button_pressed && system_state.armed) { - if (counter < DISARM_COUNTER_THRESHOLD) { + + if (counter < ARM_COUNTER_THRESHOLD) { counter++; - } else if (counter == DISARM_COUNTER_THRESHOLD) { + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; @@ -147,6 +159,8 @@ safety_check_button(void *arg) } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; + } else if (system_state.vector_flight_mode_ok) { + pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } /* Turn the LED on if we have a 1 at the current bit position */ @@ -173,7 +187,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_use_fmu) { + if (!system_state.mixer_fmu_available) { failsafe = !failsafe; } else { diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c89388be1..f84e5df8a 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -109,7 +109,7 @@ sbus_input(void) * frame transmission time is ~2ms. * * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, + * and if an interval of more than 3ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -117,7 +117,6 @@ sbus_input(void) * if we didn't drop bytes... */ now = hrt_absolute_time(); - if ((now - last_rx_time) > 3000) { if (partial_frame_count > 0) { sbus_frame_drops++; @@ -134,7 +133,6 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) goto out; - last_rx_time = now; /* @@ -146,7 +144,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -157,7 +155,7 @@ sbus_input(void) out: /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' */ return (now - last_frame_time) < 200000; } @@ -179,23 +177,23 @@ struct sbus_bit_pick { uint8_t mask; uint8_t lshift; }; -static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { - /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, - /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, - /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, - /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, - /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, - /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, - /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, - /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, - /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, - /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, - /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { +/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, +/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, +/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} }, +/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, +/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, +/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} }, +/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, +/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} }, +/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} }, +/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} }, +/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} }, +/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} }, +/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} }, +/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} }, +/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} }, +/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } }; static void @@ -219,8 +217,8 @@ sbus_decode(hrt_abstime frame_time) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -238,7 +236,6 @@ sbus_decode(hrt_abstime frame_time) value |= piece; } } - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ system_state.rc_channel_data[channel] = (value / 2) + 998; } @@ -255,7 +252,7 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channels = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + system_state.rc_channels_timestamp = frame_time; /* trigger an immediate report to the FMU */ system_state.fmu_report_due = true; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index cc74ae705..1546fb56d 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -69,58 +69,100 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); - -PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA +// PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC9_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC10_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC11_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC13_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); @@ -129,12 +171,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); -PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); -PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); + +PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); + +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); - +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index c099509a7..bf53bb9a9 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -96,6 +96,8 @@ #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ +#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) + /** * Sensor app start / stop handling function * @@ -124,7 +126,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ #if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ @@ -170,7 +172,7 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - float ex[_rc_max_chan_count]; + // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -185,11 +187,27 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_mode_sw; + + int rc_map_manual_override_sw; + int rc_map_auto_mode_sw; + + int rc_map_manual_mode_sw; + int rc_map_sas_mode_sw; + int rc_map_rtl_sw; + int rc_map_offboard_ctrl_mode_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; float rc_scale_roll; float rc_scale_pitch; float rc_scale_yaw; + float rc_scale_flaps; float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -200,9 +218,11 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t ex[_rc_max_chan_count]; + // param_t ex[_rc_max_chan_count]; param_t rc_type; + param_t rc_demix; + param_t gyro_offset[3]; param_t accel_offset[3]; param_t accel_scale[3]; @@ -213,11 +233,27 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_mode_sw; + + param_t rc_map_manual_override_sw; + param_t rc_map_auto_mode_sw; + + param_t rc_map_manual_mode_sw; + param_t rc_map_sas_mode_sw; + param_t rc_map_rtl_sw; + param_t rc_map_offboard_ctrl_mode_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; param_t rc_scale_roll; param_t rc_scale_pitch; param_t rc_scale_yaw; + param_t rc_scale_flaps; param_t battery_voltage_scaling; } _parameter_handles; /**< handles for interesting parameters */ @@ -381,22 +417,43 @@ Sensors::Sensors() : sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles.dz[i] = param_find(nbuf); - /* channel exponential gain */ - sprintf(nbuf, "RC%d_EXP", i + 1); - _parameter_handles.ex[i] = param_find(nbuf); + // /* channel exponential gain */ + // sprintf(nbuf, "RC%d_EXP", i + 1); + // _parameter_handles.ex[i] = param_find(nbuf); } _parameter_handles.rc_type = param_find("RC_TYPE"); + // _parameter_handles.rc_demix = param_find("RC_DEMIX"); + + /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); + _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); + _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); + _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -453,10 +510,10 @@ Sensors::~Sensors() int Sensors::parameters_update() { - const unsigned int nchans = 8; + bool rc_valid = true; /* rc values */ - for (unsigned int i = 0; i < nchans; i++) { + for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); @@ -473,25 +530,27 @@ Sensors::parameters_update() if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { warnx("Failed getting dead zone for chan %d", i); } - if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { - warnx("Failed getting exponential gain for chan %d", i); - } + // if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { + // warnx("Failed getting exponential gain for chan %d", i); + // } _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + if (!isfinite(_parameters.scaling_factor[i]) || + _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || + _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + + /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; + rc_valid = false; } } - /* update RC function mappings */ - _rc.function[0] = _parameters.rc_map_throttle - 1; - _rc.function[1] = _parameters.rc_map_roll - 1; - _rc.function[2] = _parameters.rc_map_pitch - 1; - _rc.function[3] = _parameters.rc_map_yaw - 1; - _rc.function[4] = _parameters.rc_map_mode_sw - 1; + /* handle wrong values */ + if (!rc_valid) + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { @@ -511,8 +570,44 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { + warnx("Failed getting override sw chan index"); + } + if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { + warnx("Failed getting auto mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { + warnx("Failed getting manual mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { + warnx("Failed getting rtl sw chan index"); + } + if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { + warnx("Failed getting sas mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { + warnx("Failed getting offboard control mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { + warnx("Failed getting mode aux 1 index"); + } + if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { + warnx("Failed getting mode aux 2 index"); + } + if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { + warnx("Failed getting mode aux 3 index"); + } + if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { + warnx("Failed getting mode aux 4 index"); + } + if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { + warnx("Failed getting mode aux 5 index"); } if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { @@ -524,6 +619,31 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { warnx("Failed getting rc scaling for yaw"); } + if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { + warnx("Failed getting rc scaling for flaps"); + } + + /* update RC function mappings */ + _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[ROLL] = _parameters.rc_map_roll - 1; + _rc.function[PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; + _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + + _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; + _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; + _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; + + _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -917,8 +1037,23 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; - /* get a copy first, to prevent altering values */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* initialize to default values */ + manual_control.roll = NAN; + manual_control.pitch = NAN; + manual_control.yaw = NAN; + manual_control.throttle = NAN; + + manual_control.manual_mode_switch = NAN; + manual_control.manual_sas_switch = NAN; + manual_control.return_to_launch_switch = NAN; + manual_control.auto_offboard_input_switch = NAN; + + manual_control.flaps = NAN; + manual_control.aux1 = NAN; + manual_control.aux2 = NAN; + manual_control.aux3 = NAN; + manual_control.aux4 = NAN; + manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) @@ -966,44 +1101,99 @@ Sensors::ppm_poll() manual_control.timestamp = rc_input.timestamp; /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } - + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); /* * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, * so reverse sign. */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); /* throttle input */ manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* override switch input */ + manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); + /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + if (_rc.function[MANUAL_MODE] >= 0) { + manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + } + + if (_rc.function[SAS_MODE] >= 0) { + manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + } - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + if (_rc.function[RTL] >= 0) { + manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + } + + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* check if ready for publishing */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + } else { + /* advertise the rc topic */ + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + /* check if ready for publishing */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } } } @@ -1051,7 +1241,7 @@ Sensors::task_main() struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); - raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; @@ -1070,27 +1260,6 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - /* advertise the manual_control topic */ - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - - /* advertise the rc topic */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - } - /* wakeup source(s) */ struct pollfd fds[1]; diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 9105d83cb..32df446d9 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -55,3 +55,92 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } + +void rot2quat(const float R[9], float Q[4]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + int32_t idx; + + /* conversion of rotation matrix to quaternion + * choose the largest component to begin with */ + q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; + q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; + q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; + q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; + + idx = 0; + + if (q0_2 < q1_2) { + q0_2 = q1_2; + + idx = 1; + } + + if (q0_2 < q2_2) { + q0_2 = q2_2; + idx = 2; + } + + if (q0_2 < q3_2) { + q0_2 = q3_2; + idx = 3; + } + + q0_2 = sqrtf(q0_2); + + /* solve for the remaining three components */ + if (idx == 0) { + q1_2 = q0_2; + q2_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[6] - R[2]) / 4.0F / q0_2; + q0_2 = (R[1] - R[3]) / 4.0F / q0_2; + } else if (idx == 1) { + q2_2 = q0_2; + q1_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[6] + R[2]) / 4.0F / q0_2; + } else if (idx == 2) { + q3_2 = q0_2; + q1_2 = (R[6] - R[2]) / 4.0F / q0_2; + q2_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[7] + R[5]) / 4.0F / q0_2; + } else { + q1_2 = (R[1] - R[3]) / 4.0F / q0_2; + q2_2 = (R[6] + R[2]) / 4.0F / q0_2; + q3_2 = (R[7] + R[5]) / 4.0F / q0_2; + } + + /* return values */ + Q[0] = q1_2; + Q[1] = q2_2; + Q[2] = q3_2; + Q[3] = q0_2; +} + +void quat2rot(const float Q[4], float R[9]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + + memset(&R[0], 0, 9U * sizeof(float)); + + q0_2 = Q[0] * Q[0]; + q1_2 = Q[1] * Q[1]; + q2_2 = Q[2] * Q[2]; + q3_2 = Q[3] * Q[3]; + + R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; + R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); + R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); + R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); + R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; + R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); + R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); + R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); + R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; +}
\ No newline at end of file diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index dc383e770..3a24ca9b7 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,6 +44,8 @@ #include <float.h> #include <stdint.h> +#define CONSTANTS_ONE_G 9.80665f + __BEGIN_DECLS /** @@ -56,6 +58,26 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); +/** + * Converts a 3 x 3 rotation matrix to an unit quaternion. + * + * All orientations are expressed in NED frame. + * + * @param R rotation matrix to convert + * @param Q quaternion to write back to + */ +__EXPORT void rot2quat(const float R[9], float Q[4]); + +/** + * Converts an unit quaternion to a 3 x 3 rotation matrix. + * + * All orientations are expressed in NED frame. + * + * @param Q quaternion to convert + * @param R rotation matrix to write back to + */ +__EXPORT void quat2rot(const float Q[4], float R[9]); + __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 9aeab1dcc..25d19f9ad 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -55,6 +55,8 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : Mixer(control_cb, cb_handle), diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 3dd6dfcf7..cf9debe6c 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -56,6 +56,8 @@ #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lib_lowprintf(fmt "\n", ##args) #define CW (-1.0f) #define CCW (1.0f) @@ -213,9 +215,11 @@ unsigned MultirotorMixer::mix(float *outputs, unsigned space) { float roll = get_control(0, 0) * _roll_scale; + //lib_lowprintf("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); float pitch = get_control(0, 1) * _pitch_scale; float yaw = get_control(0, 2) * _yaw_scale; float thrust = get_control(0, 3); + //lib_lowprintf("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float max = 0.0f; float fixup_scale; @@ -272,7 +276,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (outputs[i] < _deadband) outputs[i] = _deadband; - return 0; + return _rotor_count; } void diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25..49315cdc9 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -183,3 +183,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo return pid->last_output; } + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index b51afef9e..64d668867 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -72,6 +72,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT void pid_reset_integral(PID_t *pid); #endif /* PID_H_ */ diff --git a/apps/systemlib/scheduling_priorities.h b/apps/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/apps/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/sched.h> + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index aad2c4d9b..40278c56c 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_controls_effective.h * * Actuator control topics - mixer inputs. * diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560af..bbe429073 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 1368cb716..261a8a4ad 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -48,29 +48,33 @@ * @{ */ -enum MANUAL_CONTROL_MODE -{ - MANUAL_CONTROL_MODE_DIRECT = 0, - MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, - MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, - MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - struct manual_control_setpoint_s { uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ + + float manual_override_switch; /**< manual override mode (mandatory) */ + float auto_mode_switch; /**< auto mode switch (mandatory) */ + + /** + * Any of the channels below may not be available and be set to NaN + * to indicate that it does not contain valid data. + */ + float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ + float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ + float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ + float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float override_mode_switch; + float flaps; /**< flap position */ - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ }; /**< manual control inputs */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index fef6ef2b3..9dd54df91 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,6 +50,13 @@ * @{ */ +/** + * The number of RC channel inputs supported. + * Current (Q1/2013) radios support up to 18 channels, + * leaving at a sane value of 14. + */ +#define RC_CHANNELS_MAX 14 + /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -62,14 +69,18 @@ enum RC_CHANNELS_FUNCTION PITCH = 2, YAW = 3, OVERRIDE = 4, - FUNC_0 = 5, - FUNC_1 = 6, - FUNC_2 = 7, - FUNC_3 = 8, - FUNC_4 = 9, - FUNC_5 = 10, - FUNC_6 = 11, - RC_CHANNELS_FUNCTION_MAX = 12 + AUTO_MODE = 5, + MANUAL_MODE = 6, + SAS_MODE = 7, + RTL = 8, + OFFBOARD_MODE = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; struct rc_channels_s { @@ -78,14 +89,13 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_FUNCTION_MAX]; - uint8_t chan_count; /**< maximum number of valid channels */ + } chan[RC_CHANNELS_MAX]; + uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - uint8_t function[RC_CHANNELS_FUNCTION_MAX]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ - bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h index 3e220965d..fac571659 100644 --- a/apps/uORB/topics/vehicle_command.h +++ b/apps/uORB/topics/vehicle_command.h @@ -50,20 +50,77 @@ * @{ */ -enum PX4_CMD { - PX4_CMD_CONTROLLER_SELECTION = 1000, +/** + * Commands for commander app. + * + * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, + * but can contain additional ones. + */ +enum VEHICLE_CMD +{ + VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_ENUM_END=401, /* | */ }; +/** + * Commands for commander app. + * + * Should contain all of MAVLink's VEHICLE_CMD_RESULT values + * but can contain additional ones. + */ +enum VEHICLE_CMD_RESULT +{ + VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +}; + + struct vehicle_command_s { - float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ - float param2; /**< Parameter 2, as defined by MAVLink MAV_CMD enum. */ - float param3; /**< Parameter 3, as defined by MAVLink MAV_CMD enum. */ - float param4; /**< Parameter 4, as defined by MAVLink MAV_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink MAV_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink MAV_CMD enum. */ - float param7; /**< Parameter 7, as defined by MAVLink MAV_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by MAV_CMD enum. */ + float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ + float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ + float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ + float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ + float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ + uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 23172d7cf..ccd00c52f 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, VEHICLE_MODE_FLAG_TEST_ENABLED = 2, @@ -87,16 +87,48 @@ enum VEHICLE_MODE_FLAG { }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */ - VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */ - VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ + VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ }; -enum VEHICLE_ATTITUDE_MODE { - VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */ - VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */ +enum VEHICLE_MANUAL_CONTROL_MODE { + VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ + VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +}; + +enum VEHICLE_MANUAL_SAS_MODE { + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ + VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ + VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +}; + +/** + * Should match 1:1 MAVLink's MAV_TYPE ENUM + */ +enum VEHICLE_TYPE { + VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET=9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + VEHICLE_TYPE_KITE=17, /* Kite | */ + VEHICLE_TYPE_ENUM_END=18, /* | */ }; /** @@ -115,10 +147,9 @@ struct vehicle_status_s commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - - // uint8_t mode; - + enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ @@ -164,9 +195,12 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; -// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ - bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - + bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool flag_local_position_valid; + bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_valid_launch_position; /**< indicates a valid launch position */ }; /** |