diff options
Diffstat (limited to 'src/modules/mavlink_onboard')
-rw-r--r-- | src/modules/mavlink_onboard/mavlink.c | 126 | ||||
-rw-r--r-- | src/modules/mavlink_onboard/mavlink_receiver.c | 2 | ||||
-rw-r--r-- | src/modules/mavlink_onboard/orb_topics.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink_onboard/util.h | 3 |
4 files changed, 73 insertions, 62 deletions
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index cb6d6b16a..35d53b880 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,83 +274,89 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (armed->armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - 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; + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// 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: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } + } /** @@ -362,8 +368,10 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct vehicle_control_mode_s control_mode; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -431,10 +439,10 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0acbea675..2d406fb9f 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -328,4 +328,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -}
\ No newline at end of file +} diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..f01f09e12 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,9 +52,11 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_rc_input.h> @@ -69,7 +71,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..c6a2e52bf 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); |