fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'multirotor' of github.com:cvg/Firmware_Private into fixedwing_l1

Browse Source
This commit is contained in:
Lorenz Meier 2013-08-28 11:14:31 +02:00
commit 7fa2b9c91a
16 changed files with 450 additions and 476 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ROMFS/px4fmu_common/init.d/rcS
View File

@ -108,25 +108,17 @@ then
nshterm /dev/ttyACM0 &
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
# Upgrade PX4IO firmware
#
if [ -f /fs/microsd/px4io.bin ]
if px4io update
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
if [ -d /fs/microsd ]
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
fi
echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log
fi
# Allow IO to safely kick back to app
usleep 200000
fi
#

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=24
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4

2
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=24
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4

8
src/drivers/px4io/px4io.cpp
View File

@ -2096,10 +2096,10 @@ px4io_main(int argc, char *argv[])
fn[1] = nullptr;
} else {
fn[0] = "/fs/microsd/px4io.bin";
fn[1] = "/etc/px4io.bin";
fn[2] = "/fs/microsd/px4io2.bin";
fn[3] = "/etc/px4io2.bin";
fn[0] = "/etc/extras/px4io-v2_default.bin";
fn[1] = "/etc/extras/px4io-v1_default.bin";
fn[2] = "/fs/microsd/px4io.bin";
fn[3] = "/fs/microsd/px4io2.bin";
fn[4] = nullptr;
}

41
src/drivers/px4io/px4io_uploader.cpp
View File

@ -82,6 +82,27 @@ PX4IO_Uploader::upload(const char *filenames[])
#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
#endif
/* allow an early abort and look for file first */
for (unsigned i = 0; filenames[i] != nullptr; i++) {
_fw_fd = open(filenames[i], O_RDONLY);
if (_fw_fd < 0) {
log("failed to open %s", filenames[i]);
continue;
}
log("using firmware from %s", filenames[i]);
filename = filenames[i];
break;
}
if (filename == NULL) {
log("no firmware found");
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
if (_io_fd < 0) {
@ -106,26 +127,6 @@ PX4IO_Uploader::upload(const char *filenames[])
return -EIO;
}
for (unsigned i = 0; filenames[i] != nullptr; i++) {
_fw_fd = open(filenames[i], O_RDONLY);
if (_fw_fd < 0) {
log("failed to open %s", filenames[i]);
continue;
}
log("using firmware from %s", filenames[i]);
filename = filenames[i];
break;
}
if (filename == NULL) {
log("no firmware found");
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);

2
src/drivers/px4io/uploader.h
View File

@ -91,7 +91,7 @@ private:
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
int get_sync(unsigned timeout = 1000);
int get_sync(unsigned timeout = 100);
int sync();
int get_info(int param, uint32_t &val);
int erase();

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

@ -147,8 +147,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
/* timout until lowlevel failsafe */
static unsigned int failsafe_lowlevel_timeout_ms;
static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
int commander_thread_main(int argc, char *argv[]);
void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
void print_reject_mode(const char *msg);
void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
void *commander_low_prio_loop(void *arg);
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
int commander_main(int argc, char *argv[])
{
@ -271,7 +273,8 @@ void usage(const char *reason)
exit(1);
}
void print_status() {
void print_status()
{
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
/* read all relevant states */
@ -279,33 +282,40 @@ void print_status() {
struct vehicle_status_s state;
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
const char* armed_str;
const char *armed_str;
switch (state.arming_state) {
case ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
default:
armed_str = "ERR: UNKNOWN STATE";
break;
case ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
default:
armed_str = "ERR: UNKNOWN STATE";
break;
}
@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
uint8_t custom_main_mode = (uint8_t) cmd->param2;
// TODO remove debug code
mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
/* reject TAKEOFF not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[])
bool arm_tune_played = false;
/* set parameters */
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");
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[])
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
/* To remember when last notification was sent */
uint64_t last_print_control_time = 0;
enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE;
bool armed_previous = false;
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[])
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
memset(&diff_pres, 0, sizeof(diff_pres));
uint64_t last_diff_pres_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
} else {
mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
}
@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX should we still allow to arm with critical battery?
//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
}
@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
/* check for first, long-term and valid GPS lock -> set home position */
float hdop_m = gps_position.eph_m;
float vdop_m = gps_position.epv_m;
/* check if GPS fix is ok */
float hdop_threshold_m = 4.0f;
float vdop_threshold_m = 8.0f;
@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
*/
if (!home_position_set && gps_position.fix_type >= 3 &&
(hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
/* copy position data to uORB home message, store it locally as well */
// TODO use global position estimate
@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[])
}
}
status.rc_signal_cutting_off = false;
status.rc_signal_lost = false;
status.rc_signal_lost_interval = 0;
transition_result_t res; // store all transitions results here
@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[])
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
(status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
(status.condition_landed && (
status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
status.navigation_state == NAVIGATION_STATE_VECTOR
))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
stick_off_counter++;
}
} else {
stick_off_counter = 0;
}
@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[])
res = check_main_state_machine(&status);
if (res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
tune_positive();
} else if (res == TRANSITION_DENIED) {
@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
/* print error message for first RC glitch and then every 5s */
if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) {
// TODO remove debug code
if (!status.rc_signal_cutting_off) {
warnx("Reason: not rc_signal_cutting_off\n");
} else {
warnx("last print time: %llu\n", last_print_control_time);
}
/* only complain if the offboard control is NOT active */
status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (status.rc_signal_lost_interval > 1000000) {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status.failsave_lowlevel = true;
status_changed = true;
}
}
}
/* END mode switch */
/* END RC state check */
// TODO check this
/* state machine update for offboard control */
if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ?
// /* decide about attitude control flag, enable in att/pos/vel */
// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
// sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
// sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
// /* decide about rate control flag, enable it always XXX (for now) */
// bool rates_ctrl_enabled = true;
// /* set up control mode */
// if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
// current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
// state_changed = true;
// }
// if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
// current_status.flag_control_rates_enabled = rates_ctrl_enabled;
// state_changed = true;
// }
// /* handle the case where offboard control signal was regained */
// if (!current_status.offboard_control_signal_found_once) {
// current_status.offboard_control_signal_found_once = true;
// /* enable offboard control, disable manual input */
// current_status.flag_control_manual_enabled = false;
// current_status.flag_control_offboard_enabled = true;
// state_changed = true;
// tune_positive();
// mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
// } else {
// if (current_status.offboard_control_signal_lost) {
// mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
// state_changed = true;
// tune_positive();
// }
// }
status.offboard_control_signal_weak = false;
status.offboard_control_signal_lost = false;
status.offboard_control_signal_lost_interval = 0;
// XXX check if this is correct
/* arm / disarm on request */
if (sp_offboard.armed && !armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
} else if (!sp_offboard.armed && armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
}
} else {
/* print error message for first offboard signal glitch and then every 5s */
if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) {
status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
last_print_control_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
/* if the signal is gone for 0.1 seconds, consider it lost */
if (status.offboard_control_signal_lost_interval > 100000) {
status.offboard_control_signal_lost = true;
status.failsave_lowlevel_start_time = hrt_absolute_time();
tune_positive();
/* kill motors after timeout */
if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) {
status.failsave_lowlevel = true;
status_changed = true;
}
}
}
}
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
status_changed = true;
} else {
status_changed = false;
}
@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
/* store old modes to detect and act on state transitions */
battery_warning_previous = status.battery_warning;
armed_previous = armed.armed;
fflush(stdout);
counter++;
@ -1343,6 +1231,7 @@ void
toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
if (armed->armed) {
/* armed, solid */
@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
led_toggle(LED_BLUE);
} else {
/* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0)
led_toggle(LED_BLUE);
}
#endif
if (changed) {
@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
/* armed, solid */
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[0] = 1000;
} else if (armed->ready_to_arm) {
for (i=0; i<3; i++) {
for (i = 0; i < 3; i++) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.duration[i*2] = 200;
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i*2] = 1000;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 800;
} else {
for (i=3; i<6; i++) {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i*2] = 100;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 100;
} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
} else {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
}
pattern.color[6*2] = RGBLED_COLOR_OFF;
pattern.duration[6*2] = 700;
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
}
if (status->condition_global_position_valid) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 1000;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 800;
} else {
for (i = 3; i < 6; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
pattern.duration[i * 2] = 100;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 100;
}
pattern.color[6 * 2] = RGBLED_COLOR_OFF;
pattern.duration[6 * 2] = 700;
}
} else {
for (i=0; i<3; i++) {
pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
pattern.duration[i*2] = 200;
pattern.color[i*2+1] = RGBLED_COLOR_OFF;
pattern.duration[i*2+1] = 200;
for (i = 0; i < 3; i++) {
pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
pattern.duration[i * 2] = 200;
pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
pattern.duration[i * 2 + 1] = 200;
}
/* not ready to arm, blink at 10Hz */
}
@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
if (status->load > 0.95f) {
if (leds_counter % 2 == 0)
led_toggle(LED_AMBER);
} else {
led_off(LED_AMBER);
}
@ -1572,70 +1474,124 @@ print_reject_arm(const char *msg)
}
transition_result_t
check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
{
transition_result_t res = TRANSITION_DENIED;
switch (current_status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
break;
case MAIN_STATE_AUTO:
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (status->main_state == MAIN_STATE_AUTO) {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */
if (local_pos->z > -5.0f || status.condition_landed) {
if (local_pos->z > -5.0f || status->condition_landed) {
res = TRANSITION_NOT_CHANGED;
break;
}
}
/* check again, state can be changed */
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
} else {
/* if not landed: */
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
if (res != TRANSITION_NOT_CHANGED) {
/* check again, state can be changed */
if (status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
} else {
/* switch to mission in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
/* not landed */
if (status->rc_signal_found_once && !status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
if (status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
} else {
/* switch to MISSION in air when no RC control */
if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
res = TRANSITION_NOT_CHANGED;
} else {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
}
}
}
}
} else {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
break;
default:
break;
} else {
/* disarmed, always switch to AUTO_READY */
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
} else {
/* manual control modes */
if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
/* switch to failsafe mode */
bool manual_control_old = control_mode->flag_control_manual_enabled;
if (!status->condition_landed) {
/* in air: try to hold position */
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
} else {
/* landed: don't try to hold position but land (if taking off) */
res = TRANSITION_DENIED;
}
if (res == TRANSITION_DENIED) {
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
}
control_mode->flag_control_manual_enabled = false;
if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
/* mark navigation state as changed to force immediate flag publishing */
set_navigation_state_changed();
res = TRANSITION_CHANGED;
}
if (res == TRANSITION_CHANGED) {
if (control_mode->flag_control_position_enabled) {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
} else {
if (status->condition_landed) {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
} else {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
}
}
}
} else {
switch (status->main_state) {
case MAIN_STATE_MANUAL:
res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
break;
case MAIN_STATE_SEATBELT:
res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
break;
case MAIN_STATE_EASY:
res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
break;
default:
break;
}
}
}
return res;
@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive();
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
tune_negative();
break;
default:
break;
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
tune_negative();
break;
default:
break;
}
}
@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg)
usleep(1000000);
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
/* reboot to bootloader */
systemreset(true);
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg)
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
break;
break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
int calib_ret = ERROR;
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_gyro_calibration(mavlink_fd);
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_mag_calibration(mavlink_fd);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
}
if (calib_ret == OK)
tune_positive();
else
tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_gyro_calibration(mavlink_fd);
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_mag_calibration(mavlink_fd);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
}
if (calib_ret == OK)
tune_positive();
else
tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
}
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
break;
}
break;
}
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg)
/* send any requested ACKs */
if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
/* send acknowledge command */
// XXX TODO
}

2
src/modules/commander/commander_params.c
View File

@ -45,7 +45,7 @@
#include <nuttx/config.h>
#include <systemlib/param/param.h>
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-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);

30
src/modules/commander/state_machine_helper.cpp
View File

@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
return ret;
}
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed)
bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
@ -276,12 +276,12 @@ check_main_state_changed()
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == current_status->navigation_state) {
if (new_navigation_state == status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_STABILIZE:
@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_ALTHOLD:
@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_VECTOR:
@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = true;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_READY:
@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_climb_rate_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
}
break;
@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_RTL:
@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed state */
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_climb_rate_enabled = true;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_auto_enabled = true;
}
break;
@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
}
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
control_mode->auto_state = current_status->navigation_state;
status->navigation_state = new_navigation_state;
control_mode->auto_state = status->navigation_state;
navigation_state_changed = true;
}
}
@ -433,6 +443,12 @@ check_navigation_state_changed()
}
}
void
set_navigation_state_changed()
{
navigation_state_changed = true;
}
/**
* Transition from one hil state to another
*/

6
src/modules/commander/state_machine_helper.h
View File

@ -58,7 +58,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
bool check_main_state_changed();
transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
bool check_navigation_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */

6
src/modules/mavlink/mavlink.c
View File

@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
**/
/* set calibration state */
if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.arming_state == ARMING_STATE_INIT
if (v_status.arming_state == ARMING_STATE_INIT
|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;

98
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -2,6 +2,7 @@
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -38,6 +39,7 @@
* Implementation of multirotor attitude control main loop.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@ -63,6 +65,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
@ -74,8 +77,6 @@
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[])
memset(&offboard_sp, 0, sizeof(offboard_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_status_s status;
memset(&status, 0, sizeof(status));
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[])
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
/*
* Do not rate-limit the loop to prevent aliasing
@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[])
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);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */
bool control_yaw_position = true;
bool reset_yaw_sp = true;
bool failsafe_first_time = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
param_get(failsafe_throttle_handle, &failsafe_throttle);
while (!thread_should_exit) {
@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* update parameters */
param_get(failsafe_throttle_handle, &failsafe_throttle);
}
/* only run controller if attitude changed */
@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
}
/* get a local copy of status */
orb_check(status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), status_sub, &status);
}
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[])
/* manual input */
if (control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (control_mode.failsave_highlevel) {
failsafe_first_time = false;
if (att_sp.thrust < 0.1f) {
/* no thrust, don't try to control yaw */
rates_sp.yaw = 0.0f;
control_yaw_position = false;
if (!control_mode.flag_control_velocity_enabled) {
/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
}
if (!control_mode.flag_control_climb_rate_enabled) {
/* don't touch throttle in modes with altitude hold, it's handled by position controller.
*
* 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 > min_takeoff_throttle) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
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
* the current orientation.
*/
if (failsafe_first_time) {
if (status.condition_landed) {
/* reset yaw setpoint if on ground */
reset_yaw_sp = true;
}
} else {
failsafe_first_time = true;
/* only move yaw setpoint if manual input is != 0 and not landed */
/* only move yaw setpoint if manual input is != 0 */
if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
/* control yaw rate */
control_yaw_position = false;
@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[])
} else {
control_yaw_position = true;
}
}
if (!control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
if (!control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle;
}
if (!control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
att_sp.thrust = manual.throttle;
}
}
/* reset yaw setpint to current position if needed */
@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.timestamp = hrt_absolute_time();
/* publish the controller output */
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else {
@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[])
}
} else {
if (!control_mode.flag_control_auto_enabled) {
/* no control, try to stay on place */
if (!control_mode.flag_control_velocity_enabled) {
/* no velocity control, reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
}
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}

59
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* force reprojection of global setpoint after manual mode */
global_pos_sp_reproject = true;
} else {
/* non-manual mode, use global setpoint */
} else if (control_mode.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_pos = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z);
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) {
if (reset_auto_pos) {
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = local_pos.z;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
reset_auto_pos = false;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z);
}
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_pos = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (global_pos_sp_reproject) {
/* update global setpoint projection */
global_pos_sp_reproject = false;
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
local_pos_sp.yaw = global_pos_sp.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
}
}
reset_auto_pos = true;
}
@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
global_pos_sp_reproject = true;
}
/* reset setpoints after non-manual modes */
/* reset setpoints after AUTO mode */
reset_sp_xy = true;
reset_sp_z = true;
} else {
/* no control, loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
}
reset_sp_z = true;
} else {
/* in air: hold altitude */
if (reset_sp_z) {
reset_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_sp_xy) {
reset_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
local_pos_sp_valid = true;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y);
}
}
}
/* publish local position setpoint */

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds[6].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {

4
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -80,9 +80,7 @@ struct vehicle_control_mode_s
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */
bool flag_control_auto_enabled; // TEMP
uint8_t auto_state; // TEMP navigation state for AUTO modes
};

13
src/modules/uORB/topics/vehicle_status.h
View File

@ -174,8 +174,6 @@ struct vehicle_status_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED
main_state_t main_state; /**< main state machine */
navigation_state_t navigation_state; /**< navigation state machine */
@ -207,22 +205,13 @@ struct vehicle_status_s
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */
bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */
uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */
bool rc_signal_lost; /**< true if RC reception lost */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel;
bool preflight_calibration;
bool system_emergency;
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;