fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 18:59:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'multirotor' of github.com:cvg/Firmware_Private into multirotor

Browse Source
This commit is contained in:
Lorenz Meier 2013-08-24 13:42:10 +02:00
commit 8df6acbfaf
5 changed files with 89 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/modules/commander/commander.cpp
View File

@ -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
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
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);
} else {
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);
}
}
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 {
/* always switch to loiter in air when no RC control */
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
/* 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 {
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 {
res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
}
break;
default:

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

@ -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
View File

@ -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
View File

@ -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)
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;

2
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@ -247,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
}
rates_sp->thrust = att_sp->thrust;
//need to update the timestamp now that we've touched rates_sp
rates_sp->timestamp = hrt_absolute_time();
motor_skip_counter++;
}