mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 15:57:36 +08:00
offboard setpoint support
This commit is contained in:
Anton Babushkin
+145
-106
@@ -119,6 +119,7 @@ extern struct system_load_s system_load;
|
||||
|
||||
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
|
||||
#define RC_TIMEOUT 100000
|
||||
#define OFFBOARD_TIMEOUT 200000
|
||||
#define DIFFPRESS_TIMEOUT 2000000
|
||||
|
||||
#define PRINT_INTERVAL 5000000
|
||||
@@ -699,9 +700,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
memset(&sp_man, 0, sizeof(sp_man));
|
||||
|
||||
/* Subscribe to offboard control data */
|
||||
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
struct offboard_control_setpoint_s sp_offboard;
|
||||
memset(&sp_offboard, 0, sizeof(sp_offboard));
|
||||
int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
struct offboard_control_setpoint_s offboard_sp;
|
||||
memset(&offboard_sp, 0, sizeof(offboard_sp));
|
||||
|
||||
/* Subscribe to global position */
|
||||
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
@@ -815,10 +816,10 @@ int commander_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
|
||||
}
|
||||
|
||||
orb_check(sp_offboard_sub, &updated);
|
||||
orb_check(offboard_sp_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp);
|
||||
}
|
||||
|
||||
orb_check(sensor_sub, &updated);
|
||||
@@ -1040,126 +1041,164 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
/* ignore RC signals if in offboard control mode */
|
||||
if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
|
||||
/* start RC input check */
|
||||
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status.rc_signal_found_once) {
|
||||
status.rc_signal_found_once = true;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
|
||||
/* start RC input check */
|
||||
if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status.rc_signal_found_once) {
|
||||
status.rc_signal_found_once = true;
|
||||
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
|
||||
status_changed = true;
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
|
||||
status_changed = true;
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
status.rc_signal_lost = false;
|
||||
status.rc_signal_lost = false;
|
||||
|
||||
transition_result_t res; // store all transitions results here
|
||||
transition_result_t res; // store all transitions results here
|
||||
|
||||
/* arm/disarm by RC */
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
/* arm/disarm by RC */
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
|
||||
* do it only for rotary wings */
|
||||
if (status.is_rotary_wing &&
|
||||
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
|
||||
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
|
||||
* do it only for rotary wings */
|
||||
if (status.is_rotary_wing &&
|
||||
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
|
||||
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
|
||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
|
||||
stick_off_counter = 0;
|
||||
|
||||
} else {
|
||||
stick_off_counter++;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
|
||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
res = arming_state_transition(&status, &safety, new_arming_state, &armed);
|
||||
stick_off_counter = 0;
|
||||
}
|
||||
|
||||
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
|
||||
if (status.arming_state == ARMING_STATE_STANDBY &&
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
if (safety.safety_switch_available && !safety.safety_off) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
|
||||
} else if (status.main_state != MAIN_STATE_MANUAL) {
|
||||
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||
|
||||
} else {
|
||||
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
|
||||
}
|
||||
|
||||
stick_on_counter = 0;
|
||||
|
||||
} else {
|
||||
stick_on_counter++;
|
||||
}
|
||||
|
||||
} else {
|
||||
stick_on_counter = 0;
|
||||
}
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
if (status.arming_state == ARMING_STATE_ARMED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
|
||||
}
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
}
|
||||
|
||||
/* fill current_status according to mode switches */
|
||||
check_mode_switches(&sp_man, &status);
|
||||
|
||||
/* evaluate the main state machine */
|
||||
res = check_main_state_machine(&status);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
|
||||
tune_positive();
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
stick_off_counter++;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
|
||||
status.rc_signal_lost = true;
|
||||
status_changed = true;
|
||||
stick_off_counter = 0;
|
||||
}
|
||||
|
||||
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
|
||||
if (status.arming_state == ARMING_STATE_STANDBY &&
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
if (safety.safety_switch_available && !safety.safety_off) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
|
||||
} else if (status.main_state != MAIN_STATE_MANUAL) {
|
||||
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
|
||||
|
||||
} else {
|
||||
res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
|
||||
}
|
||||
|
||||
stick_on_counter = 0;
|
||||
|
||||
} else {
|
||||
stick_on_counter++;
|
||||
}
|
||||
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
|
||||
|
||||
} else {
|
||||
stick_on_counter = 0;
|
||||
}
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
if (status.arming_state == ARMING_STATE_ARMED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
|
||||
}
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
}
|
||||
|
||||
/* fill current_status according to mode switches */
|
||||
check_mode_switches(&sp_man, &status);
|
||||
|
||||
/* evaluate the main state machine */
|
||||
res = check_main_state_machine(&status);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
|
||||
tune_positive();
|
||||
|
||||
} else if (res == TRANSITION_DENIED) {
|
||||
/* DENIED here indicates bug in the commander */
|
||||
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
|
||||
status.rc_signal_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
|
||||
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
|
||||
status.set_nav_state = NAV_STATE_RTL;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
} else if (status.main_state != MAIN_STATE_SEATBELT) {
|
||||
res = main_state_transition(&status, MAIN_STATE_SEATBELT);
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
|
||||
status.set_nav_state = NAV_STATE_RTL;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
} else if (status.main_state != MAIN_STATE_SEATBELT) {
|
||||
res = main_state_transition(&status, MAIN_STATE_SEATBELT);
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
|
||||
}
|
||||
mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* check offboard signal */
|
||||
if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) {
|
||||
if (!status.offboard_control_signal_found_once) {
|
||||
status.offboard_control_signal_found_once = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
|
||||
status_changed = true;
|
||||
|
||||
} else {
|
||||
if (status.offboard_control_signal_lost) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained");
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
status.offboard_control_signal_lost = false;
|
||||
|
||||
if (status.main_state == MAIN_STATE_OFFBOARD) {
|
||||
if (offboard_sp.armed && !armed.armed) {
|
||||
if (!safety.safety_switch_available || safety.safety_off) {
|
||||
transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
|
||||
}
|
||||
}
|
||||
} else if (!offboard_sp.armed && armed.armed) {
|
||||
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
|
||||
transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.offboard_control_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST");
|
||||
status.offboard_control_signal_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
|
||||
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
|
||||
@@ -1264,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
led_deinit();
|
||||
buzzer_deinit();
|
||||
close(sp_man_sub);
|
||||
close(sp_offboard_sub);
|
||||
close(offboard_sp_sub);
|
||||
close(local_position_sub);
|
||||
close(global_position_sub);
|
||||
close(gps_sub);
|
||||
|
||||
@@ -97,6 +97,7 @@ static mavlink_status_t status;
|
||||
static struct vehicle_vicon_position_s vicon_position;
|
||||
static struct vehicle_command_s vcmd;
|
||||
static struct offboard_control_setpoint_s offboard_control_sp;
|
||||
static struct vehicle_attitude_setpoint_s att_sp;
|
||||
|
||||
struct vehicle_global_position_s hil_global_pos;
|
||||
struct vehicle_local_position_s hil_local_pos;
|
||||
@@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1;
|
||||
static orb_advert_t flow_pub = -1;
|
||||
|
||||
static orb_advert_t offboard_control_sp_pub = -1;
|
||||
static orb_advert_t att_sp_pub = -1;
|
||||
static orb_advert_t vicon_position_pub = -1;
|
||||
static orb_advert_t telemetry_status_pub = -1;
|
||||
|
||||
@@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg)
|
||||
/* Handle quadrotor motor setpoints */
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
|
||||
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
|
||||
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
|
||||
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint;
|
||||
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint);
|
||||
|
||||
if (mavlink_system.sysid < 4) {
|
||||
|
||||
@@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg)
|
||||
uint8_t ml_mode = 0;
|
||||
bool ml_armed = false;
|
||||
|
||||
switch (quad_motors_setpoint.mode) {
|
||||
switch (quad_swarm_setpoint.mode) {
|
||||
case 0:
|
||||
ml_armed = false;
|
||||
break;
|
||||
@@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg)
|
||||
break;
|
||||
}
|
||||
|
||||
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
|
||||
offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
|
||||
|
||||
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
|
||||
if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
|
||||
ml_armed = false;
|
||||
}
|
||||
|
||||
@@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg)
|
||||
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
|
||||
} else {
|
||||
/* Publish */
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
|
||||
}
|
||||
|
||||
if (v_status.main_state == MAIN_STATE_OFFBOARD) {
|
||||
/* in offboard mode also publish setpoint directly */
|
||||
switch (offboard_control_sp.mode) {
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
att_sp.roll_body = offboard_control_sp.p1;
|
||||
att_sp.pitch_body = offboard_control_sp.p2;
|
||||
att_sp.yaw_body = offboard_control_sp.p3;
|
||||
att_sp.thrust = offboard_control_sp.p4;
|
||||
att_sp.R_valid = false;
|
||||
att_sp.q_d_valid = false;
|
||||
att_sp.q_e_valid = false;
|
||||
att_sp.roll_reset_integral = false;
|
||||
|
||||
/* check if topic has to be advertised */
|
||||
if (att_sp_pub <= 0) {
|
||||
att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
|
||||
} else {
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -72,6 +72,7 @@
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/state_table.h>
|
||||
@@ -149,8 +150,9 @@ private:
|
||||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _offboard_mission_sub; /**< notification of offboard mission updates */
|
||||
int _onboard_mission_sub; /**< notification of onboard mission updates */
|
||||
int _onboard_mission_sub; /**< notification of onboard mission updates */
|
||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||
int _offboard_sub; /**< offboard control setpoint subscription */
|
||||
|
||||
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
|
||||
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
||||
@@ -162,6 +164,7 @@ private:
|
||||
struct home_position_s _home_pos; /**< home position for RTL */
|
||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
||||
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
|
||||
struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
@@ -383,6 +386,7 @@ Navigator::Navigator() :
|
||||
_offboard_mission_sub(-1),
|
||||
_onboard_mission_sub(-1),
|
||||
_capabilities_sub(-1),
|
||||
_offboard_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_triplet_pub(-1),
|
||||
@@ -422,6 +426,7 @@ Navigator::Navigator() :
|
||||
memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
|
||||
|
||||
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
||||
memset(&_offboard, 0, sizeof(_offboard));
|
||||
|
||||
nav_states_str[0] = "NONE";
|
||||
nav_states_str[1] = "READY";
|
||||
@@ -591,6 +596,7 @@ Navigator::task_main()
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
|
||||
_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
@@ -1523,12 +1529,49 @@ Navigator::publish_control_mode()
|
||||
case MAIN_STATE_OFFBOARD:
|
||||
_control_mode.flag_control_offboard_enabled = true;
|
||||
_control_mode.flag_control_manual_enabled = false;
|
||||
_control_mode.flag_control_rates_enabled = false;
|
||||
_control_mode.flag_control_attitude_enabled = false;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = false;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
|
||||
switch (_offboard.mode) {
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = false;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = false;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = false;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = true;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = true;
|
||||
break;
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
|
||||
_control_mode.flag_control_rates_enabled = true;
|
||||
_control_mode.flag_control_attitude_enabled = true;
|
||||
_control_mode.flag_control_altitude_enabled = true;
|
||||
_control_mode.flag_control_climb_rate_enabled = true;
|
||||
_control_mode.flag_control_position_enabled = true;
|
||||
_control_mode.flag_control_velocity_enabled = true;
|
||||
break;
|
||||
default:
|
||||
_control_mode.flag_control_rates_enabled = false;
|
||||
_control_mode.flag_control_attitude_enabled = false;
|
||||
_control_mode.flag_control_altitude_enabled = false;
|
||||
_control_mode.flag_control_climb_rate_enabled = false;
|
||||
_control_mode.flag_control_position_enabled = false;
|
||||
_control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
Reference in New Issue
Block a user