zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge branch 'seatbelt_multirotor_new' of github.com:PX4/Firmware into multirotor

sbg
Lorenz Meier 12 years ago
parent
6865c1f599 5e9b508ea0
commit
c42c28ebf4
4 changed files with 84 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 68
      src/modules/commander/commander.cpp
  2. 29
      src/modules/commander/px4_custom_mode.h
  3. 24
      src/modules/mavlink/mavlink.c
  4. 5
      src/modules/mavlink/mavlink_receiver.cpp

68
src/modules/commander/commander.cpp
Unescape View File

@ -322,7 +322,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe @@ -322,7 +322,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
uint32_t custom_mode = (uint32_t) cmd->param2;
union px4_custom_mode custom_mode;
custom_mode.data_float = cmd->param2;
// TODO remove debug code
mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode);
@ -355,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe @@ -355,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}
@ -1588,43 +1589,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v @@ -1588,43 +1589,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
break;
case MAIN_STATE_AUTO:
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
res = TRANSITION_NOT_CHANGED;
} else {
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't act while taking off */
res = TRANSITION_NOT_CHANGED;
} else {
/* if not landed: */
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
if (current_status->condition_landed) {
/* if landed: transitions only to AUTO_READY are allowed */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
// TRANSITION_DENIED is not possible here
break;
} else {
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* if not landed: */
if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
/* act depending on switches when manual control enabled */
if (current_status->return_switch == RETURN_SWITCH_RETURN) {
/* RTL */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
/* MISSION */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
} else {
/* LOITER */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
}
} else {
/* always switch to loiter in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
} else {
/* always switch to loiter in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
}
}
}
} else {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
break;
default:

29
src/modules/commander/px4_custom_mode.h
Unescape View File

@ -8,11 +8,30 @@ @@ -8,11 +8,30 @@
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO,
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_AUTO,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
};
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
#endif /* PX4_CUSTOM_MODE_H_ */

24
src/modules/mavlink/mavlink.c
Unescape View File

@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u @@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
/* main state */
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
}
*mavlink_custom_mode = custom_mode.data;
/**
* Set mavlink state

5
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -71,6 +71,7 @@ @@ -71,6 +71,7 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
__BEGIN_DECLS
@ -196,9 +197,11 @@ handle_message(mavlink_message_t *msg) @@ -196,9 +197,11 @@ handle_message(mavlink_message_t *msg)
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = new_mode.custom_mode;
vcmd.param2 = custom_mode.data_float;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;

Write Preview
Loading…
Cancel
Save