diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.c | 44 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.c | 19 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 7 |
3 files changed, 64 insertions, 6 deletions
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 7853d2e79..edcdf7e54 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -79,6 +79,7 @@ #include <uORB/topics/actuator_safety.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> +#include <uORB/topics/safety.h> #include <mavlink/mavlink_log.h> #include <drivers/drv_led.h> @@ -1229,6 +1230,10 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ + + /* XXX do we need this? */ + //current_status.flag_safety_present = false; // XXX for now just set sensors as initialized current_status.condition_system_sensors_initialized = true; @@ -1352,7 +1357,6 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; - // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1374,6 +1378,39 @@ int commander_thread_main(int argc, char *argv[]) /* Get current values */ bool new_data; + + /* 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 (!safety.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; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + orb_check(sp_man_sub, &new_data); if (new_data) { @@ -1408,7 +1445,6 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } - /* update parameters */ @@ -1686,7 +1722,9 @@ int commander_thread_main(int argc, char *argv[]) // state_changed = true; // } - if (orb_check(gps_sub, &new_data)) { + orb_check(ORB_ID(vehicle_gps_position), &new_data); + if (new_data) { + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 394ee67cc..87aad6270 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -54,9 +54,25 @@ #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" +#include "commander.h" + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { - + + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ @@ -717,7 +733,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } - ///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ // //int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 8d8536090..b015c4efe 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -49,8 +49,13 @@ #include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_control_mode.h> + void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +bool is_multirotor(const struct vehicle_status_s *current_status); + +bool is_rotary_wing(const struct vehicle_status_s *current_status); + //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -59,4 +64,4 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); -#endif /* STATE_MACHINE_HELPER_H_ */
\ No newline at end of file +#endif /* STATE_MACHINE_HELPER_H_ */ |