diff options
Diffstat (limited to 'src/modules/mavlink/mavlink.c')
-rw-r--r-- | src/modules/mavlink/mavlink.c | 114 |
1 files changed, 54 insertions, 60 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index de78cd139..15ba8860d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -195,95 +195,88 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* reset MAVLink mode bitfield */ *mavlink_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (armed.armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *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; + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - } else { - *mavlink_state = MAV_STATE_UNINIT; - } + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; + /** + * Set mavlink state + **/ - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; + /* set calibration state */ + if (v_status.flag_preflight_calibration) { - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - break; + *mavlink_state = MAV_STATE_CALIBRATING; - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + } else if (v_status.flag_system_emergency) { - case SYSTEM_STATE_EMCY_LANDING: *mavlink_state = MAV_STATE_EMERGENCY; - break; - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + *mavlink_state = MAV_STATE_ACTIVE; - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + + *mavlink_state = MAV_STATE_STANDBY; + + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + + *mavlink_state = MAV_STATE_UNINIT; + } else { + + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } } @@ -579,6 +572,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } @@ -689,7 +683,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&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); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); |