diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-21 12:55:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-21 12:55:41 +0200 |
commit | 082074f99196f8c936e21740a84b6738cb87875e (patch) | |
tree | 92863b8532541d9dcfc8a160a7f659c5e1475b35 /apps/commander/commander.c | |
parent | 572efc3383c6c98769efc65806a6d2e596787c4d (diff) | |
download | px4-firmware-082074f99196f8c936e21740a84b6738cb87875e.tar.gz px4-firmware-082074f99196f8c936e21740a84b6738cb87875e.tar.bz2 px4-firmware-082074f99196f8c936e21740a84b6738cb87875e.zip |
Completely implemented offboard control
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 249 |
1 files changed, 175 insertions, 74 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f5e143066..011a7be97 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -65,7 +65,8 @@ #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.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_command.h> #include <uORB/topics/subsystem_info.h> @@ -83,7 +84,8 @@ #include <drivers/drv_baro.h> - +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 */ #include <arch/board/up_cpuload.h> extern struct system_load_s system_load; @@ -94,14 +96,15 @@ extern struct system_load_s system_load; #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define STICK_ON_OFF_LIMIT 7500 -#define STICK_THRUST_RANGE 20000 +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define GPS_FIX_TYPE_2D 2 #define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT 50 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) /* File descriptors */ static int leds; @@ -114,6 +117,8 @@ static orb_advert_t stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; +static unsigned int failsafe_lowlevel_timeout_ms; + 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 */ @@ -1048,6 +1053,10 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + /* welcome user */ printf("[commander] I am in command now!\n"); @@ -1119,10 +1128,15 @@ int commander_thread_main(int argc, char *argv[]) int gps_quality_good_counter = 0; - /* Subscribe to RC data */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + 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; @@ -1141,11 +1155,15 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; while (!thread_should_exit) { /* Get current values */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + 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); @@ -1256,10 +1274,10 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* Check if last transition deserved an audio event */ -#warning This code depends on state that is no longer? maintained -#if 0 - trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); -#endif +// #warning This code depends on state that is no longer? maintained +// #if 0 +// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); +// #endif /* only check gps fix if we are outdoor */ // if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { @@ -1314,89 +1332,169 @@ int commander_thread_main(int argc, char *argv[]) /* end: check gps */ - /* Start RC state check */ - bool prev_lost = current_status.rc_signal_lost; + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once) { + /* Start RC state check */ + bool prev_lost = current_status.rc_signal_lost; - if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* quadrotor specific logic - check against system type in the future */ + /* 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 + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; - int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; - int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; - int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } - /* check if left stick is in lower left position --> switch to standby state */ - if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + /* 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; - } else { - stick_off_counter++; - stick_on_counter = 0; + } else { + stick_on_counter++; + 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); - /* check if left stick is in lower right position --> arm */ - if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - stick_on_counter++; - stick_off_counter = 0; + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } - } - //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 (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { - update_state_machine_mode_manual(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."); + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); + } - } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* Publish RC signal */ - - - /* handle the case where RC signal was regained */ - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; + static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + mavlink_log_critical(mavlink_fd, "[commander] 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) */ + current_status.rc_signal_cutting_off = true; + current_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 (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + } - } else { - static uint64_t last_print_time = 0; - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); - last_print_time = hrt_absolute_time(); + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } } - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_cutting_off = true; - current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp; - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true; + /* Check if this is the first loss or first gain*/ + if ((!prev_lost && current_status.rc_signal_lost) || + (prev_lost && !current_status.rc_signal_lost)) { + /* publish change */ + publish_armed_status(¤t_status); + } } - /* Check if this is the first loss or first gain*/ - if ((!prev_lost && current_status.rc_signal_lost) || - prev_lost && !current_status.rc_signal_lost) { - /* publish rc lost */ - publish_armed_status(¤t_status); - } + + /* End mode switch */ /* END RC state check */ + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + /* set up control mode */ + if (current_status.flag_control_attitude_enabled != + (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) { + current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE); + state_changed = true; + } + + if (current_status.flag_control_rates_enabled != + (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) { + current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES); + state_changed = true; + } + + /* handle the case where RC 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; + + mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME."); + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!"); + state_changed = true; + } + } + + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_system_armed) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + /* switch to stabilized mode = takeoff */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } else if (!sp_offboard.armed && current_status.flag_system_armed) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + } + + } else { + static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the RC control is NOT active */ + mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL 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) */ + current_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 (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + current_status.failsave_lowlevel = true; + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) { + state_changed = true; + } + } + } + } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -1411,8 +1509,10 @@ int commander_thread_main(int argc, char *argv[]) } /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + state_changed = false; } /* Store old modes to detect and act on state transitions */ @@ -1430,7 +1530,8 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(rc_sub); + close(sp_man_sub); + close(sp_offboard_sub); close(gps_sub); close(sensor_sub); |