fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 01:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/io_fixes' into new_state_machine

Browse Source 
Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c
	src/modules/commander/state_machine_helper.h
	src/modules/px4iofirmware/mixer.cpp
	src/modules/uORB/topics/actuator_controls.h
	src/modules/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes
2013-06-18 15:35:26 +02:00
parent 34c197c7cc c3a8f177b6
commit 202792294a
13 changed files with 343 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.c
+41 -3
View File
@@ -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, &param_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, &current_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);
src/modules/commander/state_machine_helper.c
+17 -2
View File
@@ -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)
src/modules/commander/state_machine_helper.h
+6 -1
View File
@@ -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_ */
#endif /* STATE_MACHINE_HELPER_H_ */