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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Trent Lukaczyk
2015-02-05 20:19:04 -08:00
parent 6798aee13a 7e6198b3dd
commit 531eaa2314
215 changed files with 10958 additions and 2458 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+1 -1
View File
@@ -592,7 +592,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
R = R_decl * R_body;
/* copy rotation matrix */
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
src/modules/bottle_drop/bottle_drop.cpp
+5
View File
@@ -57,8 +57,13 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
src/modules/commander/accelerometer_calibration.cpp
+8 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd)
accel_scale.z_scale = accel_T_rotated(2, 2);
/* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
|| param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
|| param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
|| param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
|| param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
|| param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR;
}
if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
res = ERROR;
}
}
src/modules/commander/commander.cpp
+152 -148
View File
@@ -74,6 +74,10 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
@@ -364,31 +368,31 @@ void print_status()
const char *armed_str;
switch (state.arming_state) {
case ARMING_STATE_INIT:
case vehicle_status_s::ARMING_STATE_INIT:
armed_str = "INIT";
break;
case ARMING_STATE_STANDBY:
case vehicle_status_s::ARMING_STATE_STANDBY:
armed_str = "STANDBY";
break;
case ARMING_STATE_ARMED:
case vehicle_status_s::ARMING_STATE_ARMED:
armed_str = "ARMED";
break;
case ARMING_STATE_ARMED_ERROR:
case vehicle_status_s::ARMING_STATE_ARMED_ERROR:
armed_str = "ARMED_ERROR";
break;
case ARMING_STATE_STANDBY_ERROR:
case vehicle_status_s::ARMING_STATE_STANDBY_ERROR:
armed_str = "STANDBY_ERROR";
break;
case ARMING_STATE_REBOOT:
case vehicle_status_s::ARMING_STATE_REBOOT:
armed_str = "REBOOT";
break;
case ARMING_STATE_IN_AIR_RESTORE:
case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE:
armed_str = "IN_AIR_RESTORE";
break;
@@ -411,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
@@ -448,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
// Transition the arming state
@@ -458,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
}
}
}
@@ -521,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -546,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
unsigned int mav_goto = (cmd->param1 + 0.5f);
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -664,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL;
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -794,41 +798,41 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
const char *main_states_str[MAIN_STATE_MAX];
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL";
main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL";
main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[ARMING_STATE_INIT] = "INIT";
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED";
arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR";
arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT";
arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
const char *nav_states_str[NAVIGATION_STATE_MAX];
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -849,10 +853,10 @@ int commander_thread_main(int argc, char *argv[])
status.condition_landed = true; // initialize to safe value
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.nav_state = NAVIGATION_STATE_MANUAL;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
@@ -865,7 +869,7 @@ int commander_thread_main(int argc, char *argv[])
status.data_link_lost = true;
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
// XXX for now just set sensors as initialized
@@ -1139,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[])
}
/* disable manual override for all systems that rely on electronic stabilization */
if (status.system_type == VEHICLE_TYPE_COAXIAL ||
status.system_type == VEHICLE_TYPE_HELICOPTER ||
status.system_type == VEHICLE_TYPE_TRICOPTER ||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR ||
status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
@@ -1155,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
@@ -1306,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(safety), safety_sub, &safety);
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
@@ -1340,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
@@ -1512,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
@@ -1520,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[])
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1531,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1545,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1658,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST 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.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 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);
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -1684,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1692,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if (status.main_state != MAIN_STATE_MANUAL) {
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1715,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[])
}
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
} else {
@@ -1853,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state != MAIN_STATE_MANUAL &&
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1877,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[])
/* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state == MAIN_STATE_ACRO ||
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
@@ -1972,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[])
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
/* play tune on battery warning or failsafe */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
@@ -2068,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
bool set_normal_color = false;
/* set mode */
if (status_local->arming_state == ARMING_STATE_ARMED) {
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2087,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (set_normal_color) {
/* set color */
if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
rgbled_set_color(RGBLED_COLOR_AMBER);
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
} else {
if (status_local->condition_local_position_valid) {
@@ -2145,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* if offboard is set allready by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
}
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -2162,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_ACRO);
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else {
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2189,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->posctl_switch != SWITCH_POS_ON) {
if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
print_reject_mode(status_local, "ALTCTL");
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO
if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2215,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
} else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2231,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_LOITER");
} else {
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2240,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -2248,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
}
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
@@ -2279,11 +2283,11 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {
case NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
@@ -2295,7 +2299,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2307,7 +2311,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2319,12 +2323,12 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RCRECOVER:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2336,7 +2340,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2348,7 +2352,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -2361,7 +2365,7 @@ set_control_mode()
break;
case NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2374,7 +2378,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
@@ -2387,7 +2391,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@@ -2400,7 +2404,7 @@ set_control_mode()
control_mode.flag_control_termination_enabled = true;
break;
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
@@ -2599,7 +2603,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
@@ -2663,7 +2667,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
src/modules/commander/commander_helper.cpp
+6 -6
View File
@@ -72,16 +72,16 @@ static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL);
}
static int buzzer = -1;
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+173 -173
View File
@@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transtion, the requested state to transition to and finally the expected
// machine state after transition.
@@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
@@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby error",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to reboot",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to init",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to standby error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to reboot",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to standby",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to armed error",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed error to standby error",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY_ERROR,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby error to reboot",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to armed",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to reboot",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// Safety switch arming tests
{ "transition: standby to armed, no safety switch",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to armed, safety switch off",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// standby error
{ "transition: armed error to standby error requested standby",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby to armed error",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED_ERROR,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to init",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_INIT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to reboot",
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to armed",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to reboot",
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_REBOOT,
{ ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to armed",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to standby",
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: reboot to armed",
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: in air restore to standby",
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Sensor tests
{ "no transition: init to standby - sensors not initialized",
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_STANDBY,
{ ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{ "no transition: init to standby, safety switch on",
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
ARMING_STATE_ARMED,
{ ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status;
struct safety_s safety;
struct actuator_armed_s armed;
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
@@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
@@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
main_state_t to_state; // State to transition to
transition_result_t expected_transition_result; // Expected result from main_state_transition call
} MainTransitionTest_t;
// Bits for condition_bits
#define MTT_ALL_NOT_VALID 0
#define MTT_ROTARY_WING 1 << 0
@@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
#define MTT_LOC_POS_VALID 1 << 2
#define MTT_HOME_POS_VALID 1 << 3
#define MTT_GLOBAL_POS_VALID 1 << 4
static const MainTransitionTest_t rgMainTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
{ "transition: MANUAL to ACRO",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
{ "transition: ACRO to MANUAL",
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
{ "transition: AUTO_LOITER to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - not rotary",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: ALTCTL to MANUAL - local altitude valid",
MTT_LOC_ALT_VALID,
MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
MTT_LOC_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: POSCTL to MANUAL - local position valid, global position valid",
MTT_LOC_POS_VALID,
MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
MTT_GLOBAL_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
MTT_ROTARY_WING,
MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
MTT_ALL_NOT_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
};
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
for (size_t i=0; i<cMainTransitionTests; i++) {
const MainTransitionTest_t* test = &rgMainTransitionTests[i];
@@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
// Attempt transition
transition_result_t result = main_state_transition(&current_state, test->to_state);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
if (test->expected_transition_result == result) {
@@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void)
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
return (_tests_failed == 0);
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
src/modules/commander/gyro_calibration.cpp
+16 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -51,6 +51,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd)
int res = OK;
/* store board ID */
uint32_t mcu_id[3];
mcu_unique_id(&mcu_id[0]);
/* store last 32bit number - not unique, but unique in a given set */
param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
@@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd)
if (res == OK) {
/* set offset parameters to new values */
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
|| param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
|| param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR;
}
@@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd)
if (res == OK) {
/* set scale parameters to new values */
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
|| param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale))
|| param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
res = ERROR;
}
if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
res = ERROR;
}
}
src/modules/commander/mag_calibration.cpp
+8 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) {
/* set parameters */
if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
res = ERROR;
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
res = ERROR;
}
src/modules/commander/state_machine_helper.cpp
+95 -95
View File
@@ -76,19 +76,19 @@ static const int ERROR = -1;
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char * const state_names[ARMING_STATE_MAX] = {
static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
@@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
} else {
@@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == HIL_STATE_OFF) {
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
@@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
new_arming_state = ARMING_STATE_STANDBY_ERROR;
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
@@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
@@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
case MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
@@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_POSCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
@@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_OFFBOARD:
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
@@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_MAX:
case vehicle_status_s::MAIN_STATE_MAX:
default:
break;
}
@@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
} else {
switch (new_state) {
case HIL_STATE_OFF:
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
/* Disable publication of all attached sensors */
/* list directory */
@@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
case vehicle_status_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
case vehicle_status_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
case vehicle_status_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
@@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
@@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
@@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
@@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
case MAIN_STATE_OFFBOARD:
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = NAVIGATION_STATE_POSCTL;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_OFFBOARD;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;
src/modules/controllib/block/Block.cpp
+2 -2
View File
@@ -101,7 +101,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
void Block::updatePublications()
{
uORB::PublicationBase *pub = getPublications().getHead();
uORB::PublicationNode *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
src/modules/controllib/block/Block.hpp
+6 -6
View File
@@ -46,8 +46,8 @@
// forward declaration
namespace uORB {
class SubscriptionBase;
class PublicationBase;
class SubscriptionNode;
class PublicationNode;
}
namespace control
@@ -83,15 +83,15 @@ public:
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationBase *> & getPublications() { return _publications; }
List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
List<uORB::SubscriptionBase *> _subscriptions;
List<uORB::PublicationBase *> _publications;
List<uORB::SubscriptionNode *> _subscriptions;
List<uORB::PublicationNode *> _publications;
List<BlockParamBase *> _params;
private:
src/modules/controllib/uorb/blocks.cpp
+9 -9
View File
@@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
_actuators(ORB_ID(actuator_controls_0), &getPublications())
{
}
src/modules/controllib/uorb/blocks.hpp
+4
View File
@@ -47,6 +47,10 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+4 -3
View File
@@ -84,6 +84,7 @@
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include "estimator_22states.h"
@@ -816,7 +817,7 @@ FixedwingEstimator::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
@@ -825,7 +826,7 @@ FixedwingEstimator::task_main()
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
@@ -1416,7 +1417,7 @@ FixedwingEstimator::task_main()
math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = R(i, j);
PX4_R(_att.R, i, j) = R(i, j);
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
src/modules/fw_att_control/fw_att_control_main.cpp
+17 -12
View File
@@ -59,7 +59,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
@@ -76,6 +80,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <platforms/px4_defines.h>
/**
* Fixedwing attitude control app start / stop handling function
@@ -742,15 +747,15 @@ FixedwingAttitudeControl::task_main()
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
_att.R[0][0] = R_adapted(0, 0);
_att.R[0][1] = R_adapted(0, 1);
_att.R[0][2] = R_adapted(0, 2);
_att.R[1][0] = R_adapted(1, 0);
_att.R[1][1] = R_adapted(1, 1);
_att.R[1][2] = R_adapted(1, 2);
_att.R[2][0] = R_adapted(2, 0);
_att.R[2][1] = R_adapted(2, 1);
_att.R[2][2] = R_adapted(2, 2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
@@ -924,9 +929,9 @@ FixedwingAttitudeControl::task_main()
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+12 -10
View File
@@ -77,6 +77,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -90,6 +91,7 @@
#include <external_lgpl/tecs/tecs.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
#include <platforms/px4_defines.h>
static int _control_task = -1; /**< task handle for sensor task */
@@ -735,7 +737,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = _att.R[i][j];
_R_nb(i, j) = PX4_R(_att.R, i, j);
}
}
@@ -814,7 +816,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -961,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
@@ -972,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
@@ -985,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1138,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
if (launchDetector.launchDetectionEnabled() &&
@@ -1233,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
@@ -1325,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
@@ -1339,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+1 -1
View File
@@ -52,7 +52,7 @@ mTecs::mTecs() :
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
_status(&getPublications(), ORB_ID(tecs_status)),
_status(ORB_ID(tecs_status), &getPublications()),
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == HIL_STATE_ON);
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
}
/* check for requested subscriptions */
src/modules/mavlink/mavlink_messages.cpp
+28 -28
View File
@@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_custom_mode = 0;
/* HIL */
if (status->hil_state == HIL_STATE_ON) {
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
switch (status->nav_state) {
case NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* fallthrough */
case NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
break;
case NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case NAVIGATION_STATE_MAX:
case vehicle_status_s::NAVIGATION_STATE_MAX:
/* this is an unused case, ignore */
break;
@@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_custom_mode = custom_mode.data;
/* set system state */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) {
} else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
} else {
@@ -1431,7 +1431,7 @@ protected:
updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
updated |= _status_sub->update(&_status_time, &status);
if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@@ -2198,7 +2198,7 @@ protected:
msg.param2 = 0;
msg.param3 = 0;
/* set camera capture ON/OFF depending on arming state */
msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0;
msg.param5 = 0;
msg.param6 = 0;
msg.param7 = 0;
src/modules/mavlink/mavlink_receiver.cpp
+4 -4
View File
@@ -616,7 +616,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
@@ -798,7 +798,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
@@ -985,10 +985,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
} else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ;
int64_t dt = _time_offset - offset_ns;
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
_time_offset = offset_ns;
warnx("[timesync] Hard setting offset.");
} else {
src/modules/mc_att_control/mc_att_control_main.cpp
+6 -2
View File
@@ -67,7 +67,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
@@ -619,14 +623,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
R_sp.set(&_v_att_sp.R_body[0][0]);
R_sp.set(&_v_att_sp.R_body[0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+301
View File
@@ -0,0 +1,301 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control.cpp
* Multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*/
#include "mc_att_control.h"
#include "mc_att_control_params.h"
#include "math.h"
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
namespace mc_att_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
MulticopterAttitudeControlBase(),
_task_should_exit(false),
_actuators_0_circuit_breaker_enabled(false),
/* publications */
_att_sp_pub(nullptr),
_v_rates_sp_pub(nullptr),
_actuators_0_pub(nullptr),
_n(),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
_params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P);
_params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P);
_params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I);
_params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D);
_params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P);
_params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P);
_params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I);
_params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D);
_params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P);
_params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P);
_params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I);
_params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D);
_params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF);
_params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX);
_params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX);
_params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX);
_params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX);
_params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
_params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
_params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
/* fetch initial parameter values */
parameters_update();
/*
* do subscriptions
*/
_v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
_v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
_v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
_parameter_update = _n.subscribe<px4_parameter_update>(
&MulticopterAttitudeControl::handle_parameter_update, this, 1000);
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
_armed = _n.subscribe<px4_actuator_armed>(0);
_v_status = _n.subscribe<px4_vehicle_status>(0);
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
{
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll gains */
PX4_PARAM_GET(_params_handles.roll_p, &v);
_params.att_p(0) = v;
PX4_PARAM_GET(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v;
PX4_PARAM_GET(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
PX4_PARAM_GET(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch gains */
PX4_PARAM_GET(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
PX4_PARAM_GET(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v;
PX4_PARAM_GET(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
PX4_PARAM_GET(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw gains */
PX4_PARAM_GET(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
PX4_PARAM_GET(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
PX4_PARAM_GET(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
PX4_PARAM_GET(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff);
PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
/* manual control scale */
PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max);
PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max);
PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max);
/* acro control scale */
PX4_PARAM_GET(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
PX4_PARAM_GET(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
PX4_PARAM_GET(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg)
{
parameters_update();
}
void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
perf_begin(_loop_perf);
/* run controller on attitude changes */
static uint64_t last_run = 0;
float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
last_run = px4::get_time_micros();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
if (_v_control_mode->data().flag_control_attitude_enabled) {
control_attitude(dt);
/* publish the attitude setpoint if needed */
if (_publish_att_sp && _v_status->data().is_rotary_wing) {
_v_att_sp_mod.data().timestamp = px4::get_time_micros();
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_v_att_sp_mod);
} else {
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
}
}
/* publish attitude rates setpoint */
_v_rates_sp_mod.data().roll = _rates_sp(0);
_v_rates_sp_mod.data().pitch = _rates_sp(1);
_v_rates_sp_mod.data().yaw = _rates_sp(2);
_v_rates_sp_mod.data().thrust = _thrust_sp;
_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
if (_v_status->data().is_vtol) {
//XXX add a second publisher?
// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
} else {
_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
}
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode->data().flag_control_manual_enabled) {
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x,
_manual_control_sp->data().r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp->data().z;
/* reset yaw setpoint after ACRO */
_reset_yaw_sp = true;
/* publish attitude rates setpoint */
_v_rates_sp_mod.data().roll = _rates_sp(0);
_v_rates_sp_mod.data().pitch = _rates_sp(1);
_v_rates_sp_mod.data().yaw = _rates_sp(2);
_v_rates_sp_mod.data().thrust = _thrust_sp;
_v_rates_sp_mod.data().timestamp = px4::get_time_micros();
if (_v_rates_sp_pub != nullptr) {
_v_rates_sp_pub->publish(_v_rates_sp_mod);
} else {
if (_v_status->data().is_vtol) {
//XXX add a second publisher?
// _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
} else {
_v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
}
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
_rates_sp(0) = _v_rates_sp->data().roll;
_rates_sp(1) = _v_rates_sp->data().pitch;
_rates_sp(2) = _v_rates_sp->data().yaw;
_thrust_sp = _v_rates_sp->data().thrust;
}
}
if (_v_control_mode->data().flag_control_rates_enabled) {
control_attitude_rates(dt);
/* publish actuator controls */
_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.data().timestamp = px4::get_time_micros();
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
_actuators_0_pub->publish(_actuators);
} else {
if (_v_status->data().is_vtol) {
//XXX add a second publisher?
// _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>();
} else {
_actuators_0_pub = _n.advertise<px4_actuator_controls_0>();
}
}
}
}
}
src/modules/mc_att_control_multiplatform/mc_att_control.h
+130
View File
@@ -0,0 +1,130 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control.h
* Multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <systemlib/perf_counter.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include "mc_att_control_base.h"
class MulticopterAttitudeControl :
public MulticopterAttitudeControlBase
{
public:
/**
* Constructor
*/
MulticopterAttitudeControl();
/**
* Destructor, also kills the sensors task.
*/
~MulticopterAttitudeControl();
/* Callbacks for topics */
void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
void handle_parameter_update(const px4_parameter_update &msg);
void spin() { _n.spin(); }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
px4::Publisher<px4_vehicle_attitude_setpoint> * _att_sp_pub; /**< attitude setpoint publication */
px4::Publisher<px4_vehicle_rates_setpoint> * _v_rates_sp_pub; /**< rate setpoint publication */
px4::Publisher<px4_actuator_controls_0> * _actuators_0_pub; /**< attitude actuator controls publication */
px4::NodeHandle _n;
struct {
px4_param_t roll_p;
px4_param_t roll_rate_p;
px4_param_t roll_rate_i;
px4_param_t roll_rate_d;
px4_param_t pitch_p;
px4_param_t pitch_rate_p;
px4_param_t pitch_rate_i;
px4_param_t pitch_rate_d;
px4_param_t yaw_p;
px4_param_t yaw_rate_p;
px4_param_t yaw_rate_i;
px4_param_t yaw_rate_d;
px4_param_t yaw_ff;
px4_param_t yaw_rate_max;
px4_param_t man_roll_max;
px4_param_t man_pitch_max;
px4_param_t man_yaw_max;
px4_param_t acro_roll_max;
px4_param_t acro_pitch_max;
px4_param_t acro_yaw_max;
px4_param_t autostart_id;
} _params_handles; /**< handles for interesting parameters */
perf_counter_t _loop_perf; /**< loop performance counter */
/**
* Update our local parameter cache.
*/
int parameters_update();
};
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+310
View File
@@ -0,0 +1,310 @@
/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_base.cpp
*
* MC Attitude Controller : Control and math code
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
#include "mc_att_control_base.h"
#include <geo/geo.h>
#include <math.h>
#include <lib/mathlib/mathlib.h>
#ifdef CONFIG_ARCH_ARM
#else
#include <cmath>
using namespace std;
#endif
MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
_v_att_sp_mod(),
_v_rates_sp_mod(),
_actuators(),
_publish_att_sp(false)
{
_params.att_p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.yaw_ff = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.man_roll_max = 0.0f;
_params.man_pitch_max = 0.0f;
_params.man_yaw_max = 0.0f;
_params.acro_rate_max.zero();
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
_I.identity();
}
MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
{
}
void MulticopterAttitudeControlBase::control_attitude(float dt)
{
float yaw_sp_move_rate = 0.0f;
_publish_att_sp = false;
if (_v_control_mode->data().flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */
if (_v_control_mode->data().flag_control_velocity_enabled
|| _v_control_mode->data().flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
//XXX get rid of memcpy
memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
}
if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude stabilized mode */
_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
_publish_att_sp = true;
}
if (!_armed->data().armed) {
/* reset yaw setpoint when disarmed */
_reset_yaw_sp = true;
}
/* move yaw setpoint in all modes */
if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
if (!_v_control_mode->data().flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
* _params.man_pitch_max;
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
} else {
/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
//XXX get rid of memcpy
memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
/* reset yaw setpoint after non-manual control mode */
_reset_yaw_sp = true;
}
_thrust_sp = _v_att_sp_mod.data().thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
if (_v_att_sp_mod.data().R_valid) {
/* rotation matrix in _att_sp is valid, use it */
R_sp.set(&_v_att_sp_mod.data().R_body[0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
_v_att_sp_mod.data().yaw_body);
/* copy rotation matrix back to setpoint struct */
memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
sizeof(_v_att_sp_mod.data().R_body));
_v_att_sp_mod.data().R_valid = true;
}
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att->data().R);
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R
* (_I + e_R_cp * e_R_z_sin
+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector < 3 > e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
_params.yaw_rate_max);
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector < 3 > rates;
rates(0) = _v_att->data().rollspeed;
rates(1) = _v_att->data().pitchspeed;
rates(2) = _v_att->data().yawspeed;
/* angular rates error */
math::Vector < 3 > rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err)
+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i)
+ _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(
rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
void MulticopterAttitudeControlBase::set_actuator_controls()
{
_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
}
src/modules/mc_att_control_multiplatform/mc_att_control_base.h
+136
View File
@@ -0,0 +1,136 @@
#ifndef MC_ATT_CONTROL_BASE_H_
#define MC_ATT_CONTROL_BASE_H_
/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_base.h
*
* MC Attitude Controller : Control and math code
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
#include <px4.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <systemlib/perf_counter.h>
#include <lib/mathlib/mathlib.h>
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
using namespace px4;
class MulticopterAttitudeControlBase
{
public:
/**
* Constructor
*/
MulticopterAttitudeControlBase();
/**
* Destructor
*/
~MulticopterAttitudeControlBase();
/**
* Start the sensors task.
*
* @return OK on success.
*/
void control_attitude(float dt);
void control_attitude_rates(float dt);
void set_actuator_controls();
protected:
px4::Subscriber<px4_vehicle_attitude> *_v_att; /**< vehicle attitude */
px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */
px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp; /**< vehicle rates setpoint */
px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode; /**< vehicle control mode */
px4::Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */
px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */
px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */
px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */
px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint
that gets published eventually */
px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint
that gets published eventually*/
px4_actuator_controls_0 _actuators; /**< actuator controls */
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
math::Vector<3> _att_control; /**< attitude control vector */
math::Matrix<3, 3> _I; /**< identity matrix */
bool _reset_yaw_sp; /**< reset yaw setpoint flag */
struct {
math::Vector<3> att_p; /**< P gain for angular error */
math::Vector<3> rate_p; /**< P gain for angular rate error */
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float yaw_rate_max; /**< max yaw rate */
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
int32_t autostart_id;
} _params;
bool _publish_att_sp;
};
#endif /* MC_ATT_CONTROL_BASE_H_ */
src/modules/uORB/topics/rc_channels.h → src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+31 -57
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,68 +32,42 @@
****************************************************************************/
/**
* @file rc_channels.h
* Definition of the rc_channels uORB topic.
* @file mc_att_control_main.cpp
* Multicopter attitude controller.
*
* @deprecated DO NOT USE FOR NEW CODE
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Roman Bapst <bapstr@ethz.ch>
*
* The controller has two loops: P loop for angular error and PD loop for angular rate error.
* Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
* For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
* so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
* These two approaches fused seamlessly with weight depending on angular error.
* When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
* If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
#ifndef RC_CHANNELS_H_
#define RC_CHANNELS_H_
#include "mc_att_control.h"
#include <stdint.h>
#include "../uORB.h"
bool thread_running = false; /**< Deamon status flag */
/**
* This defines the mapping of the RC functions.
* The value assigned to the specific function corresponds to the entry of
* the channel array channels[].
*/
enum RC_CHANNELS_FUNCTION {
THROTTLE = 0,
ROLL,
PITCH,
YAW,
MODE,
RETURN,
POSCTL,
LOITER,
OFFBOARD,
ACRO,
FLAPS,
AUX_1,
AUX_2,
AUX_3,
AUX_4,
AUX_5,
PARAM_1,
PARAM_2,
PARAM_3
};
int main(int argc, char **argv)
{
px4::init(argc, argv, "mc_att_control_m");
// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
PX4_INFO("starting");
MulticopterAttitudeControl attctl;
thread_running = true;
attctl.spin();
#define RC_CHANNELS_FUNCTION_MAX 19
PX4_INFO("exiting.");
thread_running = false;
return 0;
}
/**
* @addtogroup topics
* @{
*/
struct rc_channels_s {
uint64_t timestamp; /**< Timestamp in microseconds since boot time */
uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */
float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */
uint8_t channel_count; /**< Number of valid channels */
int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */
uint8_t rssi; /**< Receive signal strength index */
bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */
}; /**< radio control channels. */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(rc_channels);
#endif
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+248
View File
@@ -0,0 +1,248 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_params.c
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_att_control_params.h"
#include <systemlib/param/param.h>
/**
* Roll P gain
*
* Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
/**
* Roll rate P gain
*
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
/**
* Roll rate I gain
*
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
/**
* Roll rate D gain
*
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
/**
* Pitch P gain
*
* Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
/**
* Pitch rate P gain
*
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
/**
* Pitch rate I gain
*
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
/**
* Pitch rate D gain
*
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
/**
* Yaw P gain
*
* Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
/**
* Yaw rate P gain
*
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
/**
* Yaw rate I gain
*
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
/**
* Yaw rate D gain
*
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
/**
* Yaw feed forward
*
* Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
/**
* Max yaw rate
*
* Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
/**
* Max acro roll rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
/**
* Max acro pitch rate
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
/**
* Max acro yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
src/modules/uORB/topics/vehicle_local_position_setpoint.h → src/modules/mc_att_control_multiplatform/mc_att_control_params.h
+30 -32
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,33 +33,33 @@
****************************************************************************/
/**
* @file vehicle_local_position_setpoint.h
* Definition of the local NED position setpoint uORB topic.
* @file mc_att_control_params.h
* Parameters for multicopter attitude controller.
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct vehicle_local_position_setpoint_s {
uint64_t timestamp; /**< timestamp of the setpoint */
float x; /**< in meters NED */
float y; /**< in meters NED */
float z; /**< in meters NED */
float yaw; /**< in radians NED -PI..+PI */
}; /**< Local position in NED frame */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_local_position_setpoint);
#endif
#define PARAM_MC_ROLL_P_DEFAULT 6.0f
#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
#define PARAM_MC_PITCH_P_DEFAULT 6.0f
#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
#define PARAM_MC_YAW_P_DEFAULT 2.0f
#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
#define PARAM_MC_YAW_FF_DEFAULT 0.5f
#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
+142
View File
@@ -0,0 +1,142 @@
/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_sim.cpp
*
* MC Attitude Controller Interface for usage in a simulator
*
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
#include "mc_att_control_sim.h"
#include <geo/geo.h>
#include <math.h>
#ifdef CONFIG_ARCH_ARM
#else
#include <cmath>
using namespace std;
#endif
MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
{
/* setup standard gains */
//XXX: make these configurable
_params.att_p(0) = 5.0;
_params.rate_p(0) = 0.05;
_params.rate_i(0) = 0.0;
_params.rate_d(0) = 0.003;
/* pitch gains */
_params.att_p(1) = 5.0;
_params.rate_p(1) = 0.05;
_params.rate_i(1) = 0.0;
_params.rate_d(1) = 0.003;
/* yaw gains */
_params.att_p(2) = 2.8;
_params.rate_p(2) = 0.2;
_params.rate_i(2) = 0.1;
_params.rate_d(2) = 0.0;
_params.yaw_rate_max = 0.5;
_params.yaw_ff = 0.5;
_params.man_roll_max = 0.6;
_params.man_pitch_max = 0.6;
_params.man_yaw_max = 0.6;
}
MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
{
}
void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
{
math::Quaternion quat;
quat(0) = (float)attitude.w();
quat(1) = (float)attitude.x();
quat(2) = (float)attitude.y();
quat(3) = (float)attitude.z();
_v_att.q[0] = quat(0);
_v_att.q[1] = quat(1);
_v_att.q[2] = quat(2);
_v_att.q[3] = quat(3);
math::Matrix<3, 3> Rot = quat.to_dcm();
_v_att.R[0][0] = Rot(0, 0);
_v_att.R[1][0] = Rot(1, 0);
_v_att.R[2][0] = Rot(2, 0);
_v_att.R[0][1] = Rot(0, 1);
_v_att.R[1][1] = Rot(1, 1);
_v_att.R[2][1] = Rot(2, 1);
_v_att.R[0][2] = Rot(0, 2);
_v_att.R[1][2] = Rot(1, 2);
_v_att.R[2][2] = Rot(2, 2);
_v_att.R_valid = true;
}
void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
{
// check if this is consistent !!!
_v_att.rollspeed = angular_rate(0);
_v_att.pitchspeed = angular_rate(1);
_v_att.yawspeed = angular_rate(2);
}
void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
{
_v_att_sp.roll_body = control_attitude_thrust_reference(0);
_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
_v_att_sp.yaw_body = control_attitude_thrust_reference(2);
_v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
// setup rotation matrix
math::Matrix<3, 3> Rot_sp;
Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
_v_att_sp.R_body[0][0] = Rot_sp(0, 0);
_v_att_sp.R_body[1][0] = Rot_sp(1, 0);
_v_att_sp.R_body[2][0] = Rot_sp(2, 0);
_v_att_sp.R_body[0][1] = Rot_sp(0, 1);
_v_att_sp.R_body[1][1] = Rot_sp(1, 1);
_v_att_sp.R_body[2][1] = Rot_sp(2, 1);
_v_att_sp.R_body[0][2] = Rot_sp(0, 2);
_v_att_sp.R_body[1][2] = Rot_sp(1, 2);
_v_att_sp.R_body[2][2] = Rot_sp(2, 2);
}
void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
{
motor_inputs(0) = _actuators.control[0];
motor_inputs(1) = _actuators.control[1];
motor_inputs(2) = _actuators.control[2];
motor_inputs(3) = _actuators.control[3];
}
src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
+97
View File
@@ -0,0 +1,97 @@
#ifndef MC_ATT_CONTROL_BASE_H_
#define MC_ATT_CONTROL_BASE_H_
/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_att_control_sim.h
*
* MC Attitude Controller Interface for usage in a simulator
*
* @author Roman Bapst <bapstr@ethz.ch>
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <lib/mathlib/mathlib.h>
#inlcude "mc_att_control_base.h"
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
class MulticopterAttitudeControlSim :
public MulticopterAttitudeControlBase
{
public:
/**
* Constructor
*/
MulticopterAttitudeControlSim();
/**
* Destructor
*/
~MulticopterAttitudeControlSim();
/* setters and getters for interface with euroc-gazebo simulator */
void set_attitude(const Eigen::Quaternion<double> attitude);
void set_attitude_rates(const Eigen::Vector3d &angular_rate);
void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
void get_mixer_input(Eigen::Vector4d &motor_inputs);
protected:
void vehicle_attitude_setpoint_poll() {};
};
#endif /* MC_ATT_CONTROL_BASE_H_ */
src/modules/uORB/topics/actuator_controls.h → src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
+54 -37
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,51 +32,68 @@
****************************************************************************/
/**
* @file actuator_controls.h
* @file mc_att_control_m_start_nuttx.cpp
*
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
*
* Each topic can be published by a single controller
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <cstdlib>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#ifndef TOPIC_ACTUATOR_CONTROLS_H
#define TOPIC_ACTUATOR_CONTROLS_H
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
}
using namespace px4;
#include <stdint.h>
#include "../uORB.h"
extern int main(int argc, char **argv);
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
int mc_att_control_m_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "usage: mc_att_control_m {start|stop|status}");
}
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
if (!strcmp(argv[1], "start")) {
/**
* @addtogroup topics
* @{
*/
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
struct actuator_controls_s {
uint64_t timestamp;
uint64_t timestamp_sample; /**< the timestamp the data this control response is based on was sampled */
float control[NUM_ACTUATOR_CONTROLS];
};
task_should_exit = false;
/**
* @}
*/
daemon_task = task_spawn_cmd("mc_att_control_m",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
3000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
ORB_DECLARE(actuator_controls_virtual_mc);
ORB_DECLARE(actuator_controls_virtual_fw);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
}
#endif
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
warnx("unrecognized command");
return 1;
}
src/modules/mc_att_control_multiplatform/module.mk
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Multirotor attitude controller (vector based, no Euler singularities)
#
MODULE_COMMAND = mc_att_control_m
SRCS = mc_att_control_main.cpp \
mc_att_control_start_nuttx.cpp \
mc_att_control.cpp \
mc_att_control_base.cpp \
mc_att_control_params.c
src/modules/mc_pos_control/mc_pos_control_main.cpp
+20 -19
View File
@@ -49,8 +49,9 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <px4.h>
#include <functional>
#include <cstdio>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
@@ -60,8 +61,7 @@
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -73,12 +73,13 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
// #include <systemlib/param/param.h>
// #include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
@@ -537,9 +538,9 @@ MulticopterPositionControl::reset_pos_sp()
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
@@ -767,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
@@ -997,10 +998,10 @@ MulticopterPositionControl::task_main()
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
@@ -1036,7 +1037,7 @@ MulticopterPositionControl::task_main()
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
@@ -1123,7 +1124,7 @@ MulticopterPositionControl::task_main()
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
@@ -1161,11 +1162,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (_att.R[2][2] > TILT_COS_MAX) {
att_comp = 1.0f / _att.R[2][2];
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
} else if (_att.R[2][2] > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1273,7 +1274,7 @@ MulticopterPositionControl::task_main()
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
@@ -1288,7 +1289,7 @@ MulticopterPositionControl::task_main()
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+949
View File
@@ -0,0 +1,949 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_pos_control.cpp
* Multicopter position controller.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "mc_pos_control.h"
#include "mc_pos_control_params.h"
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
#define MIN_DIST 0.01f
MulticopterPositionControl::MulticopterPositionControl() :
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
/* publications */
_att_sp_pub(nullptr),
_local_pos_sp_pub(nullptr),
_global_vel_sp_pub(nullptr),
_ref_alt(0.0f),
_ref_timestamp(0),
_reset_pos_sp(true),
_reset_alt_sp(true),
_mode_auto(false),
_thrust_int(),
_R()
{
memset(&_ref_pos, 0, sizeof(_ref_pos));
/*
* Do subscriptions
*/
_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
_parameter_update = _n.subscribe<px4_parameter_update>(
&MulticopterPositionControl::handle_parameter_update, this, 1000);
_manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
_armed = _n.subscribe<px4_actuator_armed>(0);
_local_pos = _n.subscribe<px4_vehicle_local_position>(0);
_pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0);
_local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0);
_global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0);
_params.pos_p.zero();
_params.vel_p.zero();
_params.vel_i.zero();
_params.vel_d.zero();
_params.vel_max.zero();
_params.vel_ff.zero();
_params.sp_offs_max.zero();
_pos.zero();
_pos_sp.zero();
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_sp_move_rate.zero();
_params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN);
_params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX);
_params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P);
_params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P);
_params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I);
_params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D);
_params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX);
_params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF);
_params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P);
_params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P);
_params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I);
_params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D);
_params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX);
_params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF);
_params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR);
_params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED);
_params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND);
/* fetch initial parameter values */
parameters_update();
_R.identity();
}
MulticopterPositionControl::~MulticopterPositionControl()
{
}
int
MulticopterPositionControl::parameters_update()
{
PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min);
PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max);
PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air);
_params.tilt_max_air = math::radians(_params.tilt_max_air);
PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed);
PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land);
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
PX4_PARAM_GET(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
PX4_PARAM_GET(_params_handles.z_p, &v);
_params.pos_p(2) = v;
PX4_PARAM_GET(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
PX4_PARAM_GET(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
PX4_PARAM_GET(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
PX4_PARAM_GET(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
PX4_PARAM_GET(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
PX4_PARAM_GET(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
PX4_PARAM_GET(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
PX4_PARAM_GET(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
PX4_PARAM_GET(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
PX4_PARAM_GET(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
return OK;
}
float
MulticopterPositionControl::scale_control(float ctl, float end, float dz)
{
if (ctl > dz) {
return (ctl - dz) / (end - dz);
} else if (ctl < -dz) {
return (ctl + dz) / (end - dz);
} else {
return 0.0f;
}
}
void
MulticopterPositionControl::update_ref()
{
if (_local_pos->data().ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp = 0.0f;
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon);
_ref_alt = _local_pos->data().ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos->data().ref_timestamp;
}
}
void
MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0)
// - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1)
// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
void
MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
}
}
void
MulticopterPositionControl::limit_pos_sp_offset()
{
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode->data().flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode->data().flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
}
void
MulticopterPositionControl::control_manual(float dt)
{
_sp_move_rate.zero();
if (_control_mode->data().flag_control_altitude_enabled) {
/* move altitude setpoint with throttle stick */
_sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode->data().flag_control_position_enabled) {
/* move position setpoint with roll/pitch stick */
_sp_move_rate(0) = _manual_control_sp->data().x;
_sp_move_rate(1) = _manual_control_sp->data().y;
}
/* limit setpoint move rate */
float sp_move_norm = _sp_move_rate.length();
if (sp_move_norm > 1.0f) {
_sp_move_rate /= sp_move_norm;
}
/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
if (_control_mode->data().flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
}
if (_control_mode->data().flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode->data().flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode->data().flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
}
void
MulticopterPositionControl::control_offboard(float dt)
{
if (_pos_sp_triplet->data().current.valid) {
if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) {
/* control position */
_pos_sp(0) = _pos_sp_triplet->data().current.x;
_pos_sp(1) = _pos_sp_triplet->data().current.y;
} else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet->data().current.vx;
_sp_move_rate(1) = _pos_sp_triplet->data().current.vy;
}
if (_pos_sp_triplet->data().current.yaw_valid) {
_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
} else if (_pos_sp_triplet->data().current.yawspeed_valid) {
_att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt;
}
if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) {
/* Control altitude */
_pos_sp(2) = _pos_sp_triplet->data().current.z;
} else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet->data().current.vz;
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
} else {
reset_pos_sp();
reset_alt_sp();
}
}
bool
MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
{
/* project center of sphere on line */
/* normalized AB */
math::Vector<3> ab_norm = line_b - line_a;
ab_norm.normalize();
math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
float cd_len = (sphere_c - d).length();
/* we have triangle CDX with known CD and CX = R, find DX */
if (sphere_r > cd_len) {
/* have two roots, select one in A->B direction from D */
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
res = d + ab_norm * dx_len;
return true;
} else {
/* have no roots, return D */
res = d;
return false;
}
}
void
MulticopterPositionControl::control_auto(float dt)
{
if (!_mode_auto) {
_mode_auto = true;
/* reset position setpoint on AUTO mode activation */
reset_pos_sp();
reset_alt_sp();
}
if (_pos_sp_triplet->data().current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt);
/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
/* find X - cross point of L1 sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than L1 radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
if (_pos_sp_triplet->data().next.valid) {
math::Vector<3> next_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon,
&next_sp.data[0], &next_sp.data[1]);
next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt);
if ((next_sp - curr_sp).length() > MIN_DIST) {
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* cos(b), b = angle pos - curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than L1 radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
}
}
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (near) {
/* L1 sphere crosses trajectory */
} else {
/* copter is too far from trajectory */
/* if copter is behind prev waypoint, go directly to prev waypoint */
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
pos_sp_s = prev_sp_s;
}
/* if copter is in front of curr waypoint, go directly to curr waypoint */
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
}
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
}
}
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet->data().current.yaw)) {
_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
}
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
}
void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg)
{
parameters_update();
}
void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg)
{
static bool reset_int_z = true;
static bool reset_int_z_manual = false;
static bool reset_int_xy = true;
static bool was_armed = false;
static uint64_t t_prev = 0;
uint64_t t = get_time_micros();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
t_prev = t;
if (_control_mode->data().flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = _control_mode->data().flag_armed;
update_ref();
if (_control_mode->data().flag_control_altitude_enabled ||
_control_mode->data().flag_control_position_enabled ||
_control_mode->data().flag_control_climb_rate_enabled ||
_control_mode->data().flag_control_velocity_enabled) {
_pos(0) = _local_pos->data().x;
_pos(1) = _local_pos->data().y;
_pos(2) = _local_pos->data().z;
_vel(0) = _local_pos->data().vx;
_vel(1) = _local_pos->data().vy;
_vel(2) = _local_pos->data().vz;
_vel_ff.zero();
_sp_move_rate.zero();
/* select control source */
if (_control_mode->data().flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode->data().flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);
}
/* fill local position setpoint */
_local_pos_sp_msg.data().timestamp = get_time_micros();
_local_pos_sp_msg.data().x = _pos_sp(0);
_local_pos_sp_msg.data().y = _pos_sp(1);
_local_pos_sp_msg.data().z = _pos_sp(2);
_local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body;
/* publish local position setpoint */
if (_local_pos_sp_pub != nullptr) {
_local_pos_sp_pub->publish(_local_pos_sp_msg);
} else {
_local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>();
}
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
_R.identity();
memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
_att_sp_msg.data().R_valid = true;
_att_sp_msg.data().roll_body = 0.0f;
_att_sp_msg.data().pitch_body = 0.0f;
_att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().thrust = 0.0f;
_att_sp_msg.data().timestamp = get_time_micros();
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_att_sp_msg);
} else {
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (!_control_mode->data().flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode->data().flag_control_position_enabled) {
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
_global_vel_sp_msg.data().vx = _vel_sp(0);
_global_vel_sp_msg.data().vy = _vel_sp(1);
_global_vel_sp_msg.data().vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub != nullptr) {
_global_vel_sp_pub->publish(_global_vel_sp_msg);
} else {
_global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>();
}
if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode->data().flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual_control_sp->data().z;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
_thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode->data().flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
_thrust_int(0) = 0.0f;
_thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int;
if (!_control_mode->data().flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode->data().flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid &&
_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode->data().flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > _params.thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > _params.thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -_params.thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = _params.thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = _params.thr_max;
}
/* update integrals */
if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) {
_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
_thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) {
_thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (_thrust_int(2) > 0.0f) {
_thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode->data().flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
_R(i, 0) = body_x(i);
_R(i, 1) = body_y(i);
_R(i, 2) = body_z(i);
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
_att_sp_msg.data().R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = _R.to_euler();
_att_sp_msg.data().roll_body = euler(0);
_att_sp_msg.data().pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode->data().flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
_R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body));
_att_sp_msg.data().R_valid = true;
_att_sp_msg.data().roll_body = 0.0f;
_att_sp_msg.data().pitch_body = 0.0f;
}
_att_sp_msg.data().thrust = thrust_abs;
_att_sp_msg.data().timestamp = get_time_micros();
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_att_sp_msg);
} else {
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
}
} else {
reset_int_z = true;
}
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+219
View File
@@ -0,0 +1,219 @@
/****************************************************************************
*
* Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_pos_control.h
* Multicopter position controller.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <px4.h>
#include <cstdio>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
// #include <poll.h>
// #include <drivers/drv_hrt.h>
// #include <arch/board/board.h>
// #include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
// #include <mavlink/mavlink_log.h>
using namespace px4;
class MulticopterPositionControl
{
public:
/**
* Constructor
*/
MulticopterPositionControl();
/**
* Destructor, also kills task.
*/
~MulticopterPositionControl();
/* Callbacks for topics */
void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
void handle_parameter_update(const px4_parameter_update &msg);
void spin() { _n.spin(); }
protected:
const float alt_ctl_dz = 0.2f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */
Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */
Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */
Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */
Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */
Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */
Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */
Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */
Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */
Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */
Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */
Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp; /**< local position */
px4_vehicle_attitude_setpoint _att_sp_msg;
px4_vehicle_local_position_setpoint _local_pos_sp_msg;
px4_vehicle_global_velocity_setpoint _global_vel_sp_msg;
px4::NodeHandle _n;
struct {
px4_param_t thr_min;
px4_param_t thr_max;
px4_param_t z_p;
px4_param_t z_vel_p;
px4_param_t z_vel_i;
px4_param_t z_vel_d;
px4_param_t z_vel_max;
px4_param_t z_ff;
px4_param_t xy_p;
px4_param_t xy_vel_p;
px4_param_t xy_vel_i;
px4_param_t xy_vel_d;
px4_param_t xy_vel_max;
px4_param_t xy_ff;
px4_param_t tilt_max_air;
px4_param_t land_speed;
px4_param_t tilt_max_land;
} _params_handles; /**< handles for interesting parameters */
struct {
float thr_min;
float thr_max;
float tilt_max_air;
float land_speed;
float tilt_max_land;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
math::Vector<3> vel_i;
math::Vector<3> vel_d;
math::Vector<3> vel_ff;
math::Vector<3> vel_max;
math::Vector<3> sp_offs_max;
} _params;
struct map_projection_reference_s _ref_pos;
float _ref_alt;
uint64_t _ref_timestamp;
bool _reset_pos_sp;
bool _reset_alt_sp;
bool _mode_auto;
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Vector<3> _sp_move_rate;
math::Vector<3> _thrust_int;
math::Matrix<3, 3> _R;
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*/
void control_update();
static float scale_control(float ctl, float end, float dz);
/**
* Update reference for local position projection
*/
void update_ref();
/**
* Reset position setpoint to current position
*/
void reset_pos_sp();
/**
* Reset altitude setpoint to current altitude
*/
void reset_alt_sp();
/**
* Check if position setpoint is too far from current position and adjust it if needed.
*/
void limit_pos_sp_offset();
/**
* Set position setpoint using manual control
*/
void control_manual(float dt);
/**
* Set position setpoint using offboard control
*/
void control_offboard(float dt);
bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
/**
* Set position setpoint for AUTO
*/
void control_auto(float dt);
/**
* Select between barometric and global (AMSL) altitudes
*/
void select_alt(bool global);
};
src/modules/uORB/topics/vehicle_global_velocity_setpoint.h → src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp
+24 -24
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,31 +32,32 @@
****************************************************************************/
/**
* @file vehicle_global_velocity_setpoint.h
* Definition of the global velocity setpoint uORB topic.
* @file mc_pos_control_main.cpp
* Multicopter position controller.
*
* The controller has two loops: P loop for position error and PID loop for velocity error.
* Output of velocity controller is thrust vector that splitted to thrust direction
* (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
#include "mc_pos_control.h"
#include "../uORB.h"
bool thread_running = false; /**< Deamon status flag */
/**
* @addtogroup topics
* @{
*/
int main(int argc, char **argv)
{
px4::init(argc, argv, "mc_pos_control_m");
struct vehicle_global_velocity_setpoint_s {
float vx; /**< in m/s NED */
float vy; /**< in m/s NED */
float vz; /**< in m/s NED */
}; /**< Velocity setpoint in NED frame */
PX4_INFO("starting");
MulticopterPositionControl posctl;
thread_running = true;
posctl.spin();
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_global_velocity_setpoint);
#endif
PX4_INFO("exiting.");
thread_running = false;
return 0;
}
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
+212
View File
@@ -0,0 +1,212 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_pos_control_params.c
* Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <px4_defines.h>
#include "mc_pos_control_params.h"
#include <systemlib/param/param.h>
/**
* Minimum thrust
*
* Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
/**
* Maximum thrust
*
* Limit max allowed thrust.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
/**
* Proportional gain for vertical position error
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
/**
* Proportional gain for vertical velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
/**
* Integral gain for vertical velocity error
*
* Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
/**
* Differential gain for vertical velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
/**
* Maximum vertical velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL).
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
/**
* Proportional gain for horizontal position error
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
/**
* Proportional gain for horizontal velocity error
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
/**
* Integral gain for horizontal velocity error
*
* Non-zero value allows to resist wind.
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
/**
* Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
/**
* Maximum horizontal velocity
*
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
/**
* Maximum tilt angle in air
*
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
/**
* Maximum tilt during landing
*
* Limits maximum tilt angle on landing.
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
/**
* Landing descend rate
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
src/modules/uORB/topics/actuator_armed.h → src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
+22 -30
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,38 +33,29 @@
****************************************************************************/
/**
* @file actuator_armed.h
*
* Actuator armed topic
* @file mc_pos_control_params.h
* Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef TOPIC_ACTUATOR_ARMED_H
#define TOPIC_ACTUATOR_ARMED_H
#pragma once
#include <stdint.h>
#include "../uORB.h"
#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
#define PARAM_MPC_Z_P_DEFAULT 1.0f
#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
#define PARAM_MPC_Z_FF_DEFAULT 0.5f
#define PARAM_MPC_XY_P_DEFAULT 1.0f
#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
#define PARAM_MPC_XY_FF_DEFAULT 0.5f
#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
/**
* @addtogroup topics
* @{
*/
/** global 'actuator output is live' control. */
struct actuator_armed_s {
uint64_t timestamp; /**< Microseconds since system boot */
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(actuator_armed);
#endif
src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
+99
View File
@@ -0,0 +1,99 @@
/****************************************************************************
*
* Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_pos_control_m_start_nuttx.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <cstdlib>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
extern bool thread_running;
int daemon_task; /**< Handle of deamon task / thread */
namespace px4
{
bool task_should_exit = false;
}
using namespace px4;
extern int main(int argc, char **argv);
extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]);
int mc_pos_control_m_main(int argc, char *argv[])
{
if (argc < 1) {
errx(1, "usage: mc_pos_control_m {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
task_should_exit = false;
daemon_task = task_spawn_cmd("mc_pos_control_m",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
3000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
task_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
warnx("unrecognized command");
return 1;
}
src/modules/mc_pos_control_multiplatform/module.mk
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build multicopter position controller
#
MODULE_COMMAND = mc_pos_control_m
SRCS = mc_pos_control_main.cpp \
mc_pos_control_start_nuttx.cpp \
mc_pos_control.cpp \
mc_pos_control_params.c
src/modules/navigator/loiter.cpp
+1 -1
View File
@@ -82,7 +82,7 @@ Loiter::on_activation()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
}
src/modules/navigator/mission.cpp
+2 -2
View File
@@ -397,7 +397,7 @@ Mission::set_mission_items()
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
set_mission_finished();
@@ -476,7 +476,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
src/modules/navigator/mission_block.cpp
+5 -5
View File
@@ -194,25 +194,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;
default:
sp->type = SETPOINT_TYPE_POSITION;
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
}
src/modules/navigator/navigator_main.cpp
+17 -20
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -217,7 +217,7 @@ Navigator::vehicle_status_update()
{
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
/* in case the commander is not be running */
_vstatus.arming_state = ARMING_STATE_STANDBY;
_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
}
@@ -247,9 +247,6 @@ Navigator::task_main_trampoline(int argc, char *argv[])
void
Navigator::task_main()
{
/* inform about start */
warnx("Initializing..");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
@@ -263,7 +260,7 @@ Navigator::task_main()
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
mavlink_log_info(_mavlink_fd, "No geofence set");
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
@@ -419,25 +416,25 @@ Navigator::task_main()
/* Do stuff according to navigation state set by commander */
switch (_vstatus.nav_state) {
case NAVIGATION_STATE_MANUAL:
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_LAND:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
@@ -445,11 +442,11 @@ Navigator::task_main()
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
_pos_sp_triplet_published_invalid_once = false;
@@ -459,11 +456,11 @@ Navigator::task_main()
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
src/modules/position_estimator_inav/position_estimator_inav_main.c
+14 -9
View File
@@ -53,6 +53,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -68,6 +72,7 @@
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h>
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
@@ -458,7 +463,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}
@@ -491,7 +496,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(att.R[2][2] > 0.7f) &&
(PX4_R(att.R, 2, 2) > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
@@ -528,15 +533,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -559,7 +564,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
}
}
@@ -568,7 +573,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
@@ -940,7 +945,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
@@ -965,7 +970,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
src/modules/sdlog2/sdlog2.c
+5 -1
View File
@@ -70,6 +70,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
src/modules/sensors/sensor_params.c
+361 -25
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,20 +36,27 @@
*
* Parameters defined by the sensors task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@px4.io>
* @author Thomas Gubler <thomas@px4.io>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* ID of the board this parameter set was calibrated on.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_BOARD_ID, 0);
/**
* ID of the Gyro that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
/**
* Gyro X-axis offset
@@ -58,7 +65,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0f);
/**
* Gyro Y-axis offset
@@ -67,7 +74,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f);
/**
* Gyro Z-axis offset
@@ -76,7 +83,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
* @max 5.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
@@ -85,7 +92,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
@@ -94,7 +101,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
@@ -103,14 +110,14 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_GYRO0_ZSCALE, 1.0f);
/**
* ID of Magnetometer the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
/**
* Magnetometer X-axis offset
@@ -119,7 +126,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0f);
/**
* Magnetometer Y-axis offset
@@ -128,7 +135,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0f);
/**
* Magnetometer Z-axis offset
@@ -137,78 +144,407 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0f);
/**
* Magnetometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0f);
/**
* Magnetometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
/**
* Magnetometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
/**
* ID of the Accelerometer that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
/**
* Accelerometer X-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f);
/**
* ID of the Gyro that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
/**
* Gyro X-axis offset
*
* @min -10.0
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0f);
/**
* Gyro Y-axis offset
*
* @min -10.0
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f);
/**
* Gyro Z-axis offset
*
* @min -5.0
* @max 5.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO1_ZSCALE, 1.0f);
/**
* ID of Magnetometer the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
/**
* Magnetometer X-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0f);
/**
* Magnetometer Y-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0f);
/**
* Magnetometer Z-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0f);
/**
* Magnetometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0f);
/**
* Magnetometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
/**
* Magnetometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
/**
* ID of the Accelerometer that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
/**
* Accelerometer X-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f);
/**
* ID of the Gyro that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
/**
* Gyro X-axis offset
*
* @min -10.0
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0f);
/**
* Gyro Y-axis offset
*
* @min -10.0
* @max 10.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f);
/**
* Gyro Z-axis offset
*
* @min -5.0
* @max 5.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f);
/**
* Gyro X-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_XSCALE, 1.0f);
/**
* Gyro Y-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_YSCALE, 1.0f);
/**
* Gyro Z-axis scaling factor
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_GYRO2_ZSCALE, 1.0f);
/**
* ID of Magnetometer the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
/**
* Magnetometer X-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0f);
/**
* Magnetometer Y-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0f);
/**
* Magnetometer Z-axis offset
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0f);
/**
* Magnetometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0f);
/**
* Magnetometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
/**
* Magnetometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
/**
* ID of the Accelerometer that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
/**
* Accelerometer X-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0f);
/**
* Accelerometer Y-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0f);
/**
* Accelerometer Z-axis offset
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0f);
/**
* Accelerometer X-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0f);
/**
* Accelerometer Y-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
/**
* Accelerometer Z-axis scaling factor
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
/**
* Differential pressure sensor offset
src/modules/sensors/sensors.cpp
+137 -142
View File
@@ -184,13 +184,13 @@ private:
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
float get_rc_value(uint8_t func, float min_value, float max_value);
/**
* Get switch position for specified function.
*/
switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
/**
* Update paramters from RC channels if the functionality is activated and the
@@ -512,7 +512,7 @@ Sensors::Sensors() :
_hil_enabled(false),
_publishing(true),
/* subscriptions */
/* subscriptions */
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
@@ -530,7 +530,7 @@ Sensors::Sensors() :
_rc_parameter_map_sub(-1),
_manual_control_sub(-1),
/* publications */
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_actuator_group_3_pub(-1),
@@ -539,7 +539,7 @@ Sensors::Sensors() :
_airspeed_pub(-1),
_diff_pres_pub(-1),
/* performance counters */
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_mag_is_external(false),
@@ -619,29 +619,29 @@ Sensors::Sensors() :
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
_parameter_handles.gyro_offset[0] = param_find("CAL_GYRO0_XOFF");
_parameter_handles.gyro_offset[1] = param_find("CAL_GYRO0_YOFF");
_parameter_handles.gyro_offset[2] = param_find("CAL_GYRO0_ZOFF");
_parameter_handles.gyro_scale[0] = param_find("CAL_GYRO0_XSCALE");
_parameter_handles.gyro_scale[1] = param_find("CAL_GYRO0_YSCALE");
_parameter_handles.gyro_scale[2] = param_find("CAL_GYRO0_ZSCALE");
/* accel offsets */
_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
_parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF");
_parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF");
_parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE");
_parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE");
_parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE");
_parameter_handles.accel_offset[0] = param_find("CAL_ACC0_XOFF");
_parameter_handles.accel_offset[1] = param_find("CAL_ACC0_YOFF");
_parameter_handles.accel_offset[2] = param_find("CAL_ACC0_ZOFF");
_parameter_handles.accel_scale[0] = param_find("CAL_ACC0_XSCALE");
_parameter_handles.accel_scale[1] = param_find("CAL_ACC0_YSCALE");
_parameter_handles.accel_scale[2] = param_find("CAL_ACC0_ZSCALE");
/* mag offsets */
_parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF");
_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
_parameter_handles.mag_offset[0] = param_find("CAL_MAG0_XOFF");
_parameter_handles.mag_offset[1] = param_find("CAL_MAG0_YOFF");
_parameter_handles.mag_offset[2] = param_find("CAL_MAG0_ZOFF");
_parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE");
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
_parameter_handles.mag_scale[0] = param_find("CAL_MAG0_XSCALE");
_parameter_handles.mag_scale[1] = param_find("CAL_MAG0_YSCALE");
_parameter_handles.mag_scale[2] = param_find("CAL_MAG0_ZSCALE");
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -786,9 +786,11 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
}
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
@@ -813,28 +815,28 @@ Sensors::parameters_update()
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[ROLL] = _parameters.rc_map_roll - 1;
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
_rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
/* gyro offsets */
@@ -883,16 +885,19 @@ Sensors::parameters_update()
/* set px4flow rotation */
int flowfd;
flowfd = open(PX4FLOW_DEVICE_PATH, 0);
if (flowfd >= 0) {
int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
if (flowret) {
warnx("flow rotation could not be set");
int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
if (flowret) {
warnx("flow rotation could not be set");
close(flowfd);
return ERROR;
}
}
close(flowfd);
}
}
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
@@ -902,9 +907,9 @@ Sensors::parameters_update()
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
@@ -912,17 +917,20 @@ Sensors::parameters_update()
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
int barofd;
barofd = open(BARO_DEVICE_PATH, 0);
if (barofd < 0) {
warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH);
return ERROR;
} else {
int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (baroret) {
warnx("qnh could not be set");
close(barofd);
return ERROR;
}
close(barofd);
}
@@ -942,28 +950,11 @@ Sensors::accel_init()
} else {
// XXX do the check more elegantly
/* set the accel internal sampling rate to default rate */
ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the accel internal sampling rate up to at leat 1000Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#else
#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
#endif
/* set the driver to poll at default rate */
ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
}
@@ -984,31 +975,12 @@ Sensors::gyro_init()
} else {
// XXX do the check more elegantly
/* set the gyro internal sampling rate to default rate */
ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the driver to poll at default rate */
ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
/* set the gyro internal sampling rate up to at least 1000Hz */
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
ioctl(fd, GYROIOCSSAMPLERATE, 800);
}
/* set the driver to poll at 1000Hz */
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
ioctl(fd, SENSORIOCSPOLLRATE, 800);
}
#else
/* set the gyro internal sampling rate up to at least 760Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 760);
/* set the driver to poll at 760Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 760);
#endif
close(fd);
}
return OK;
@@ -1348,14 +1320,17 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
/* don't risk to feed negative airspeed into the system */
_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.indicated_airspeed_m_s = math::max(0.0f,
calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
@@ -1479,8 +1454,10 @@ Sensors::parameter_update_poll(bool forced)
}
#if 0
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0],
(int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1],
(int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100));
fflush(stdout);
usleep(5000);
@@ -1496,9 +1473,10 @@ Sensors::rc_parameter_map_poll(bool forced)
if (map_updated) {
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
@@ -1508,21 +1486,24 @@ Sensors::rc_parameter_map_poll(bool forced)
/* Set the handle by index if the index is set, otherwise use the id */
if (_rc_parameter_map.param_index[i] >= 0) {
_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
}
}
warnx("rc to parameter map updated");
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
i,
_rc_parameter_map.param_id[i],
(double)_rc_parameter_map.scale[i],
(double)_rc_parameter_map.value0[i],
(double)_rc_parameter_map.value_min[i],
(double)_rc_parameter_map.value_max[i]
);
i,
_rc_parameter_map.param_id[i],
(double)_rc_parameter_map.scale[i],
(double)_rc_parameter_map.value0[i],
(double)_rc_parameter_map.value_min[i],
(double)_rc_parameter_map.value_max[i]
);
}
}
}
@@ -1616,7 +1597,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
(diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
/* announce the airspeed if needed, just publish else */
@@ -1646,7 +1628,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
float
Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
{
if (_rc.function[func] >= 0) {
float value = _rc.channels[_rc.function[func]];
@@ -1667,41 +1649,41 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
}
switch_pos_t
Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
return manual_control_setpoint_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return SWITCH_POS_MIDDLE;
return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
} else {
return SWITCH_POS_OFF;
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return SWITCH_POS_NONE;
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
switch_pos_t
Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
return manual_control_setpoint_s::SWITCH_POS_ON;
} else {
return SWITCH_POS_OFF;
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return SWITCH_POS_NONE;
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
@@ -1709,22 +1691,23 @@ void
Sensors::set_params_from_rc()
{
static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
continue;
}
float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
param_rc_values[i] = rc_val;
float param_val = math::constrain(
_rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
_rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
_rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
_rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
param_set(_parameter_handles.rc_param[i], &param_val);
}
}
@@ -1808,10 +1791,12 @@ Sensors::rc_poll()
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(
_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(
_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
@@ -1847,24 +1832,24 @@ Sensors::rc_poll()
manual.timestamp = rc_input.timestamp_last_signal;
/* limit controls */
manual.y = get_rc_value(ROLL, -1.0, 1.0);
manual.x = get_rc_value(PITCH, -1.0, 1.0);
manual.r = get_rc_value(YAW, -1.0, 1.0);
manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
@@ -1899,6 +1884,7 @@ Sensors::rc_poll()
/* Update parameters from RC Channels (tuning with RC) if activated */
static hrt_abstime last_rc_to_param_map_time = 0;
if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) {
set_params_from_rc();
last_rc_to_param_map_time = hrt_absolute_time();
@@ -1920,22 +1906,31 @@ Sensors::task_main()
/* start individual sensors */
int ret;
ret = accel_init();
if (ret) {
goto exit_immediate;
}
ret = gyro_init();
if (ret) {
goto exit_immediate;
}
ret = mag_init();
if (ret) {
goto exit_immediate;
}
ret = baro_init();
if (ret) {
goto exit_immediate;
}
ret = adc_init();
if (ret) {
goto exit_immediate;
}
@@ -2075,7 +2070,7 @@ Sensors::start()
nullptr);
/* wait until the task is up and running or has failed */
while(_sensors_task > 0 && _task_should_exit) {
while (_sensors_task > 0 && _task_should_exit) {
usleep(100);
}
src/modules/uORB/topics/parameter_update.h → src/modules/systemlib/circuit_breaker.cpp
+18 -24
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,31 +31,25 @@
*
****************************************************************************/
/**
* @file parameter_update.h
* Notification about a parameter update.
/*
* @file circuit_breaker.c
*
* Circuit breaker parameters.
* Analog to real aviation circuit breakers these parameters
* allow to disable subsystems. They are not supported as standard
* operation procedure and are only provided for development purposes.
* To ensure they are not activated accidentally, the associated
* parameter needs to set to the key (magic).
*/
#ifndef TOPIC_PARAMETER_UPDATE_H
#define TOPIC_PARAMETER_UPDATE_H
#include <px4.h>
#include <systemlib/circuit_breaker.h>
#include <stdint.h>
#include "../uORB.h"
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)PX4_PARAM_GET_BYNAME(breaker, &val);
/**
* @addtogroup topics
* @{
*/
return (val == magic);
}
struct parameter_update_s {
/** time at which the latest parameter was updated */
uint64_t timestamp;
};
/**
* @}
*/
ORB_DECLARE(parameter_update);
#endif
src/modules/systemlib/circuit_breaker.h
+1 -1
View File
@@ -61,7 +61,7 @@
__BEGIN_DECLS
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
__END_DECLS
src/modules/systemlib/circuit_breaker.c → src/modules/systemlib/circuit_breaker_params.c
+8 -31
View File
@@ -42,8 +42,8 @@
* parameter needs to set to the key (magic).
*/
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
#include <px4.h>
#include <systemlib/circuit_breaker_params.h>
/**
* Circuit breaker for power supply check
@@ -56,7 +56,7 @@
* @max 894281
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
/**
* Circuit breaker for rate controller output
@@ -69,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
* @max 140253
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
/**
* Circuit breaker for IO safety
@@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
* @max 22027
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
/**
* Circuit breaker for airspeed sensor
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
* @max 162128
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
/**
* Circuit breaker for flight termination
@@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* @max 121212
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
/**
* Circuit breaker for engine failure detection
@@ -120,27 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @max 284953
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
/**
* Circuit breaker for gps failure detection
*
* Setting this parameter to 240024 will disable the gps failure detection.
* If the aircraft is in gps failure mode the gps failure flag will be
* set to healthy
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)param_get(param_find(breaker), &val);
return (val == magic);
}
PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
src/modules/systemlib/circuit_breaker_params.h
+7
View File
@@ -0,0 +1,7 @@
#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
src/modules/systemlib/err.h
+2 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -66,6 +66,7 @@
#define _SYSTEMLIB_ERR_H
#include <stdarg.h>
#include "visibility.h"
__BEGIN_DECLS
src/modules/systemlib/mcu_version.h
+4
View File
@@ -35,6 +35,8 @@
#include <stdint.h>
__BEGIN_DECLS
/* magic numbers from reference manual */
enum MCU_REV {
MCU_REV_STM32F4_REV_A = 0x1000,
@@ -61,3 +63,5 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
* @return The silicon revision / version number as integer
*/
__EXPORT int mcu_version(char* rev, char** revstr);
__END_DECLS
src/modules/systemlib/module.mk
+2 -1
View File
@@ -53,7 +53,8 @@ SRCS = err.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c \
circuit_breaker.c \
circuit_breaker.cpp \
circuit_breaker_params.c \
mcu_version.c
MAXOPTIMIZATION = -Os
src/modules/systemlib/perf_counter.h
+1
View File
@@ -40,6 +40,7 @@
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
#include <platforms/px4_defines.h>
/**
* Counter types.
src/modules/uORB/Publication.cpp
+5 -3
View File
@@ -49,15 +49,16 @@
#include "topics/actuator_direct.h"
#include "topics/encoders.h"
#include "topics/tecs_status.h"
#include "topics/rc_channels.h"
namespace uORB {
template<class T>
Publication<T>::Publication(
List<PublicationBase *> * list,
const struct orb_metadata *meta) :
const struct orb_metadata *meta,
List<PublicationNode *> * list) :
T(), // initialize data structure to zero
PublicationBase(list, meta) {
PublicationNode(meta, list) {
}
template<class T>
@@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
template class __EXPORT Publication<rc_channels_s>;
}
src/modules/uORB/Publication.hpp
+81 -15
View File
@@ -38,6 +38,8 @@
#pragma once
#include <assert.h>
#include <uORB/uORB.h>
#include <containers/List.hpp>
@@ -49,55 +51,112 @@ namespace uORB
* Base publication warapper class, used in list traversal
* of various publications.
*/
class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
class __EXPORT PublicationBase
{
public:
PublicationBase(
List<PublicationBase *> * list,
const struct orb_metadata *meta) :
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
*/
PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(-1) {
if (list != NULL) list->add(this);
}
void update() {
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void * data) {
if (_handle > 0) {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
orb_publish(getMeta(), getHandle(), data);
} else {
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
setHandle(orb_advertise(getMeta(), data));
}
}
virtual void *getDataVoidPtr() = 0;
/**
* Deconstructor
*/
virtual ~PublicationBase() {
orb_unsubscribe(getHandle());
}
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
orb_advert_t _handle;
};
/**
* alias class name so it is clear that the base class
* can be used by itself if desired
*/
typedef PublicationBase PublicationTiny;
/**
* The publication base class as a list node.
*/
class __EXPORT PublicationNode :
public PublicationBase,
public ListNode<PublicationNode *>
{
public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
PublicationNode(const struct orb_metadata *meta,
List<PublicationNode *> * list=nullptr) :
PublicationBase(meta) {
if (list != nullptr) list->add(this);
}
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual void update() = 0;
};
/**
* Publication wrapper class
*/
template<class T>
class Publication :
public T, // this must be first!
public PublicationBase
public PublicationNode
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param list A list interface for adding to
* list during construction
*/
Publication(List<PublicationBase *> * list,
const struct orb_metadata *meta);
Publication(const struct orb_metadata *meta,
List<PublicationNode *> * list=nullptr);
/**
* Deconstructor
**/
virtual ~Publication();
/*
* XXX
* This function gets the T struct, assuming
@@ -106,6 +165,13 @@ public:
* seem to be available
*/
void *getDataVoidPtr();
/**
* Create an update function that uses the embedded struct.
*/
void update() {
PublicationBase::update(getDataVoidPtr());
}
};
} // namespace uORB
src/modules/uORB/Subscription.cpp
+8 -12
View File
@@ -52,25 +52,18 @@
#include "topics/vehicle_local_position.h"
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/rc_channels.h"
namespace uORB
{
bool __EXPORT SubscriptionBase::updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
template<class T>
Subscription<T>::Subscription(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval) :
const struct orb_metadata *meta,
unsigned interval,
List<SubscriptionNode *> * list) :
T(), // initialize data structure to zero
SubscriptionBase(list, meta) {
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
SubscriptionNode(meta, interval, list) {
}
template<class T>
@@ -101,5 +94,8 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
template class __EXPORT Subscription<vehicle_local_position_s>;
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
template class __EXPORT Subscription<rc_channels_s>;
template class __EXPORT Subscription<vehicle_control_mode_s>;
template class __EXPORT Subscription<actuator_armed_s>;
} // namespace uORB
src/modules/uORB/Subscription.hpp
+100 -25
View File
@@ -38,10 +38,11 @@
#pragma once
#include <assert.h>
#include <uORB/uORB.h>
#include <containers/List.hpp>
namespace uORB
{
@@ -49,8 +50,7 @@ namespace uORB
* Base subscription warapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT SubscriptionBase :
public ListNode<SubscriptionBase *>
class __EXPORT SubscriptionBase
{
public:
// methods
@@ -58,23 +58,42 @@ public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
*
* @param interval The minimum interval in milliseconds
* between updates
*/
SubscriptionBase(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta) :
SubscriptionBase(const struct orb_metadata *meta,
unsigned interval=0) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
bool updated();
void update() {
/**
* Check if there is a new update.
* */
bool updated() {
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void * data) {
if (updated()) {
orb_copy(_meta, _handle, getDataVoidPtr());
orb_copy(_meta, _handle, data);
}
}
virtual void *getDataVoidPtr() = 0;
/**
* Deconstructor
*/
virtual ~SubscriptionBase() {
orb_unsubscribe(_handle);
}
@@ -90,30 +109,86 @@ protected:
};
/**
* Subscription wrapper class
* alias class name so it is clear that the base class
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionBase
typedef SubscriptionBase SubscriptionTiny;
/**
* The publication base class as a list node.
*/
class __EXPORT SubscriptionNode :
public SubscriptionBase,
public ListNode<SubscriptionNode *>
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param interval The minimum interval in milliseconds between updates
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
Subscription(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval);
SubscriptionNode(const struct orb_metadata *meta,
unsigned interval=0,
List<SubscriptionNode *> * list=nullptr) :
SubscriptionBase(meta, interval),
_interval(interval) {
if (list != nullptr) list->add(this);
}
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual void update() = 0;
// accessors
unsigned getInterval() { return _interval; }
protected:
// attributes
unsigned _interval;
};
/**
* Subscription wrapper class
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionNode
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param list A list interface for adding to
* list during construction
*/
Subscription(const struct orb_metadata *meta,
unsigned interval=0,
List<SubscriptionNode *> * list=nullptr);
/**
* Deconstructor
*/
virtual ~Subscription();
/**
* Create an update function that uses the embedded struct.
*/
void update() {
SubscriptionBase::update(getDataVoidPtr());
}
/*
* XXX
* This function gets the T struct, assuming
src/modules/uORB/objects_common.cpp
+16 -8
View File
@@ -113,8 +113,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
#include "topics/mc_virtual_rates_setpoint.h"
ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s);
#include "topics/fw_virtual_rates_setpoint.h"
ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -184,13 +186,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s);
/* actuator controls, as requested by controller */
#include "topics/actuator_controls.h"
ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
#include "topics/actuator_controls_0.h"
ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s);
#include "topics/actuator_controls_1.h"
ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s);
#include "topics/actuator_controls_2.h"
ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s);
#include "topics/actuator_controls_3.h"
ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s);
//Virtual control groups, used for VTOL operation
ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
#include "topics/actuator_controls_virtual_mc.h"
ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s);
#include "topics/actuator_controls_virtual_fw.h"
ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
src/modules/uORB/topics/airspeed.h
+1 -1
View File
@@ -40,7 +40,7 @@
#ifndef TOPIC_AIRSPEED_H_
#define TOPIC_AIRSPEED_H_
#include "../uORB.h"
#include <platforms/px4_defines.h>
#include <stdint.h>
/**
src/modules/uORB/topics/fence.h
+1 -1
View File
@@ -42,7 +42,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <platforms/px4_defines.h>
/**
* @addtogroup topics
src/modules/uORB/topics/manual_control_setpoint.h
-111
View File
@@ -1,111 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file manual_control_setpoint.h
* Definition of the manual_control_setpoint uORB topic.
*/
#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
#include <stdint.h>
#include "../uORB.h"
/**
* Switch position
*/
typedef enum {
SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
} switch_pos_t;
/**
* @addtogroup topics
* @{
*/
struct manual_control_setpoint_s {
uint64_t timestamp;
/**
* Any of the channels may not be available and be set to NaN
* to indicate that it does not contain valid data.
* The variable names follow the definition of the
* MANUAL_CONTROL mavlink message.
* The default range is from -1 to 1 (mavlink message -1000 to 1000)
* The range for the z variable is defined from 0 to 1. (The z field of
* the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
*/
float x; /**< stick position in x direction -1..1
in general corresponds to forward/back motion or pitch of vehicle,
in general a positive value means forward or negative pitch and
a negative value means backward or positive pitch */
float y; /**< stick position in y direction -1..1
in general corresponds to right/left motion or roll of vehicle,
in general a positive value means right or positive roll and
a negative value means left or negative roll */
float z; /**< throttle stick position 0..1
in general corresponds to up/down motion or thrust of vehicle,
in general the value corresponds to the demanded throttle by the user,
if the input is used for setting the setpoint of a vertical position
controller any value > 0.5 means up and any value < 0.5 means down */
float r; /**< yaw stick/twist positon, -1..1
in general corresponds to the righthand rotation around the vertical
(downwards) axis of the vehicle */
float flaps; /**< flap position */
float aux1; /**< default function: camera yaw / azimuth */
float aux2; /**< default function: camera pitch / tilt */
float aux3; /**< default function: camera trigger */
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(manual_control_setpoint);
#endif
src/modules/uORB/topics/position_setpoint_triplet.h
-116
View File
@@ -1,116 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_item_triplet.h
* Definition of the global WGS84 position setpoint uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
#define TOPIC_MISSION_ITEM_TRIPLET_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
enum SETPOINT_TYPE
{
SETPOINT_TYPE_POSITION = 0, /**< position setpoint */
SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
};
struct position_setpoint_s
{
bool valid; /**< true if setpoint is valid */
enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
float x; /**< local position setpoint in m in NED */
float y; /**< local position setpoint in m in NED */
float z; /**< local position setpoint in m in NED */
bool position_valid; /**< true if local position setpoint valid */
float vx; /**< local velocity setpoint in m/s in NED */
float vy; /**< local velocity setpoint in m/s in NED */
float vz; /**< local velocity setpoint in m/s in NED */
bool velocity_valid; /**< true if local velocity setpoint valid */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
bool yaw_valid; /**< true if yaw setpoint valid */
float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */
bool yawspeed_valid; /**< true if yawspeed setpoint valid */
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
float a_x; //**< acceleration x setpoint */
float a_y; //**< acceleration y setpoint */
float a_z; //**< acceleration z setpoint */
bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */
bool acceleration_is_force; //*< interprete acceleration as force */
};
/**
* Global position setpoint triplet in WGS84 coordinates.
*
* This are the three next waypoints (or just the next two or one).
*/
struct position_setpoint_triplet_s
{
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
unsigned nav_state; /**< report the navigation state */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(position_setpoint_triplet);
#endif
src/modules/uORB/topics/vehicle_attitude.h
-91
View File
@@ -1,91 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_attitude.h
* Definition of the attitude uORB topic.
*/
#ifndef VEHICLE_ATTITUDE_H_
#define VEHICLE_ATTITUDE_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Attitude in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*/
struct vehicle_attitude_s {
uint64_t timestamp; /**< in microseconds since system start */
/* This is similar to the mavlink message ATTITUDE, but for onboard use */
/** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
float roll; /**< Roll angle (rad, Tait-Bryan, NED) */
float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */
float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */
float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */
float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */
float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */
float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */
float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */
float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */
float rate_offsets[3]; /**< Offsets of the body angular rates from zero */
float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
float q[4]; /**< Quaternion (NED) */
float g_comp[3]; /**< Compensated gravity vector */
bool R_valid; /**< Rotation matrix valid */
bool q_valid; /**< Quaternion valid */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_attitude);
#endif
src/modules/uORB/topics/vehicle_attitude_setpoint.h
-89
View File
@@ -1,89 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_attitude_setpoint.h
* Definition of the vehicle attitude setpoint uORB topic.
*/
#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* vehicle attitude setpoint.
*/
struct vehicle_attitude_setpoint_s {
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
float roll_body; /**< body angle in NED frame */
float pitch_body; /**< body angle in NED frame */
float yaw_body; /**< body angle in NED frame */
//float body_valid; /**< Set to true if body angles are valid */
float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//! For quaternion-based attitude control
float q_d[4]; /** Desired quaternion for quaternion control */
bool q_d_valid; /**< Set to true if quaternion vector is valid */
float q_e[4]; /** Attitude error in quaternion */
bool q_e_valid; /**< Set to true if quaternion error vector is valid */
float thrust; /**< Thrust in Newton the power system should generate */
bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */
bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */
bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_attitude_setpoint);
ORB_DECLARE(mc_virtual_attitude_setpoint);
ORB_DECLARE(fw_virtual_attitude_setpoint);
#endif /* TOPIC_ARDRONE_CONTROL_H_ */
src/modules/uORB/topics/vehicle_control_mode.h
-95
View File
@@ -1,95 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_control_mode.h
* Definition of the vehicle_control_mode uORB topic.
*
* All control apps should depend their actions based on the flags set here.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef VEHICLE_CONTROL_MODE
#define VEHICLE_CONTROL_MODE
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include "vehicle_status.h"
/**
* @addtogroup topics @{
*/
/**
* state machine / state of vehicle.
*
* Encodes the complete system state and is set by the commander app.
*/
struct vehicle_control_mode_s {
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
bool flag_armed;
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
// XXX needs yet to be set by state machine helper
bool flag_system_hil_enabled;
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
bool flag_control_offboard_enabled; /**< true if offboard control should be used */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_force_enabled; /**< true if force control is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
bool flag_control_termination_enabled; /**< true if flighttermination is enabled */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_control_mode);
#endif
src/modules/uORB/topics/vehicle_global_position.h
+1 -1
View File
@@ -45,7 +45,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <platforms/px4_defines.h>
/**
* @addtogroup topics
src/modules/uORB/topics/vehicle_local_position.h
-96
View File
@@ -1,96 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_local_position.h
* Definition of the local fused NED position uORB topic.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
#define TOPIC_VEHICLE_LOCAL_POSITION_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Fused local position in NED.
*/
struct vehicle_local_position_s {
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
bool xy_valid; /**< true if x and y are valid */
bool z_valid; /**< true if z is valid */
bool v_xy_valid; /**< true if vy and vy are valid */
bool v_z_valid; /**< true if vz is valid */
/* Position in local NED frame */
float x; /**< X position in meters in NED earth-fixed frame */
float y; /**< X position in meters in NED earth-fixed frame */
float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */
/* Velocity in NED frame */
float vx; /**< Ground X Speed (Latitude), m/s in NED */
float vy; /**< Ground Y Speed (Longitude), m/s in NED */
float vz; /**< Ground Z Speed (Altitude), m/s in NED */
/* Heading */
float yaw;
/* Reference position in GPS / WGS84 frame */
bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */
double ref_lat; /**< Reference point latitude in degrees */
double ref_lon; /**< Reference point longitude in degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
/* Distance to surface */
float dist_bottom; /**< Distance to bottom surface (ground) */
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */
bool dist_bottom_valid; /**< true if distance to bottom surface is valid */
float eph;
float epv;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_local_position);
#endif
src/modules/uORB/topics/vehicle_rates_setpoint.h
-68
View File
@@ -1,68 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_rates_setpoint.h
* Definition of the vehicle rates setpoint topic
*/
#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
#define TOPIC_VEHICLE_RATES_SETPOINT_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
struct vehicle_rates_setpoint_s {
uint64_t timestamp; /**< in microseconds since system start */
float roll; /**< body angular rates in NED frame */
float pitch; /**< body angular rates in NED frame */
float yaw; /**< body angular rates in NED frame */
float thrust; /**< thrust normalized to 0..1 */
}; /**< vehicle_rates_setpoint */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_rates_setpoint);
ORB_DECLARE(mc_virtual_rates_setpoint);
ORB_DECLARE(fw_virtual_rates_setpoint);
#endif
src/modules/uORB/topics/vehicle_status.h
-263
View File
@@ -1,263 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vehicle_status.h
* Definition of the vehicle_status uORB topic.
*
* Published the state machine and the system status bitfields
* (see SYS_STATUS mavlink message), published only by commander app.
*
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app)
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef VEHICLE_STATUS_H_
#define VEHICLE_STATUS_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
*/
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_OFFBOARD,
MAIN_STATE_MAX
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
// in state_machine_helper.cpp as well.
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_ARMED_ERROR,
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX,
} arming_state_t;
typedef enum {
HIL_STATE_OFF = 0,
HIL_STATE_ON
} hil_state_t;
/**
* Navigation state, i.e. "what should vehicle do".
*/
typedef enum {
NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
/**
* Should match 1:1 MAVLink's MAV_TYPE ENUM
*/
enum VEHICLE_TYPE {
VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
VEHICLE_TYPE_KITE = 17, /* Kite | */
VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
VEHICLE_TYPE_ENUM_END = 21 /* | */
};
enum VEHICLE_BATTERY_WARNING {
VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */
VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */
};
/**
* @addtogroup topics
* @{
*/
/**
* state machine / state of vehicle.
*
* Encodes the complete system state and is set by the commander app.
*/
struct vehicle_status_s {
/* use of a counter and timestamp recommended (but not necessary) */
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
navigation_state_t nav_state; /**< set navigation state machine to specified value */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
bool failsafe; /**< true if system is in failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
bool is_rotary_wing; /**< True if system is in rotary wing configuration, so for a VTOL
this is only true while flying as a multicopter */
bool is_vtol; /**< True if the system is VTOL capable */
bool vtol_fw_permanent_stab; /**< True if vtol should stabilize attitude for fw in manual mode */
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */
bool condition_system_sensors_initialized;
bool condition_system_returned_to_home;
bool condition_auto_mission_available;
bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
bool condition_launch_position_valid; /**< indicates a valid launch position */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_local_altitude_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool condition_power_input_valid; /**< set if input power is valid */
float avionics_power_rail_voltage; /**< voltage of the avionics power rail */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */
uint8_t data_link_lost_counter; /**< counts unique data link lost events */
bool engine_failure; /** Set to true if an engine failure is detected */
bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */
bool gps_failure; /** Set to true if a gps failure is detected */
bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */
bool barometer_failure; /** Set to true if a barometer failure is detected */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
and should not be overridden by RC */
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health;
float load; /**< processor load from 0 to 1 */
float battery_voltage;
float battery_current;
float battery_remaining;
enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
uint16_t drop_rate_comm;
uint16_t errors_comm;
uint16_t errors_count1;
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
bool circuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
bool circuit_breaker_engaged_enginefailure_check;
bool circuit_breaker_engaged_gpsfailure_check;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_status);
#endif
src/modules/uORB/uORB.cpp
+75 -10
View File
@@ -56,6 +56,7 @@
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
@@ -682,9 +683,11 @@ namespace
{
ORBDevMaster *g_dev;
bool pubsubtest_passed = false;
struct orb_test {
int val;
hrt_abstime time;
};
ORB_DEFINE(orb_test, struct orb_test);
@@ -718,6 +721,46 @@ test_note(const char *fmt, ...)
return OK;
}
int pubsublatency_main(void)
{
/* poll on test topic and output latency */
float latency_integral = 0.0f;
/* wakeup source(s) */
struct pollfd fds[1];
int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
fds[0].fd = test_multi_sub;
fds[0].events = POLLIN;
struct orb_test t;
const unsigned maxruns = 10;
for (unsigned i = 0; i < maxruns; i++) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t);
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
hrt_abstime elt = hrt_elapsed_time(&t.time);
latency_integral += elt;
}
orb_unsubscribe(test_multi_sub);
warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
pubsubtest_passed = true;
return OK;
}
int
test()
{
@@ -779,8 +822,7 @@ test()
int instance0;
int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
test_note("advertised");
usleep(300000);
test_note("advertised");
int instance1;
int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
@@ -795,8 +837,7 @@ test()
if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
return test_fail("mult. pub0 fail");
test_note("published");
usleep(300000);
test_note("published");
t.val = 203;
if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
@@ -805,19 +846,19 @@ test()
/* subscribe to both topics and ensure valid data is received */
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u))
return test_fail("sub #0 copy failed: %d", errno);
if (t.val != 103)
return test_fail("sub #0 val. mismatch: %d", t.val);
if (u.val != 103)
return test_fail("sub #0 val. mismatch: %d", u.val);
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u))
return test_fail("sub #1 copy failed: %d", errno);
if (t.val != 203)
return test_fail("sub #1 val. mismatch: %d", t.val);
if (u.val != 203)
return test_fail("sub #1 val. mismatch: %d", u.val);
/* test priorities */
int prio;
@@ -833,6 +874,30 @@ test()
if (prio != ORB_PRIO_MIN)
return test_fail("prio: %d", prio);
/* test pub / sub latency */
int pubsub_task = task_spawn_cmd("uorb_latency",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(main_t)&pubsublatency_main,
nullptr);
/* give the test task some data */
while (!pubsubtest_passed) {
t.val = 303;
t.time = hrt_absolute_time();
if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
return test_fail("mult. pub0 timing fail");
/* simulate >800 Hz system operation */
usleep(1000);
}
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
return test_note("PASS");
}
src/modules/uORB/uORB.h
+10 -1
View File
@@ -144,7 +144,7 @@ typedef intptr_t orb_advert_t;
* node in /obj if required and publishes the initial data.
*
* Any number of advertisers may publish to a topic; publications are atomic
* but co-ordination between publishers is not provided by the ORB.
* but co-ordination between publishers is not provided by the ORB.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
@@ -351,4 +351,13 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT;
__END_DECLS
/* Diverse uORB header defines */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
typedef uint8_t arming_state_t;
typedef uint8_t main_state_t;
typedef uint8_t hil_state_t;
typedef uint8_t navigation_state_t;
typedef uint8_t switch_pos_t;
#endif /* _UORB_UORB_H */
src/modules/uavcan/uavcan_main.cpp
+6 -6
View File
@@ -381,7 +381,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
controls_updated = true;
@@ -393,14 +393,14 @@ int UavcanNode::run()
/*
see if we have any direct actuator updates
*/
if (_actuator_direct_sub != -1 &&
if (_actuator_direct_sub != -1 &&
(_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
!_test_in_progress) {
if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
}
memcpy(&_outputs.output[0], &_actuator_direct.values[0],
memcpy(&_outputs.output[0], &_actuator_direct.values[0],
_actuator_direct.nvalues*sizeof(float));
_outputs.noutputs = _actuator_direct.nvalues;
new_output = true;
@@ -476,7 +476,7 @@ int UavcanNode::run()
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
// Update the armed status and check that we're not locked down and motor
// Update the armed status and check that we're not locked down and motor
// test is not running
bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
@@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
int
UavcanNode::teardown()
{
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
@@ -526,7 +526,7 @@ UavcanNode::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
// the first fd used by CAN
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
src/modules/vtol_att_control/vtol_att_control_main.cpp
+4
View File
@@ -58,7 +58,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>