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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-31 11:37:32 +02:00
parent 1b3a775e1b 75dfb0d84d
commit 93a822fee4
158 changed files with 5350 additions and 1904 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+1 -1
View File
@@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd)
usleep(500 * 1000);
calibration_counter = 0;
const int maxcount = 3000;
const unsigned maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
src/modules/commander/commander.cpp
+46 -34
View File
@@ -124,7 +124,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
@@ -163,7 +163,8 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static float eph_epv_threshold = 5.0f;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -335,6 +336,7 @@ void print_status()
{
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
/* read all relevant states */
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -546,24 +548,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
} else {
/* reject parachute depoyment not armed */
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
armed_local->force_failsafe = false;
warnx("disabling failsafe");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -741,6 +738,7 @@ int commander_thread_main(int argc, char *argv[])
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
status.circuit_breaker_engaged_airspd_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -918,6 +916,11 @@ int commander_thread_main(int argc, char *argv[])
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
/* Subscribe to actuator controls (outputs) */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -977,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_changed = true;
@@ -1106,32 +1110,32 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
bool eph_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
if (global_position.eph > eph_threshold * 2.5f) {
eph_good = false;
} else {
eph_epv_good = true;
eph_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
if (global_position.eph < eph_threshold) {
eph_good = true;
} else {
eph_epv_good = false;
eph_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@@ -1161,8 +1165,8 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
if (status.condition_local_position_valid) {
if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
} else {
@@ -1170,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
if (local_position.eph < eph_epv_threshold) {
if (local_position.eph < eph_threshold) {
local_eph_good = true;
} else {
@@ -1199,13 +1203,17 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
}
}
@@ -1267,13 +1275,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
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_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* 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");
@@ -1412,8 +1420,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "arming state transition denied");
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
@@ -1441,7 +1453,7 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
@@ -1494,7 +1506,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
src/modules/commander/commander_helper.cpp
+11 -6
View File
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
}
}
float battery_remaining_estimate_voltage(float voltage, float discharged)
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
{
float ret = 0;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static param_t bat_v_load_drop_h;
static float bat_v_empty = 3.4f;
static float bat_v_full = 4.2f;
static float bat_v_load_drop = 0.06f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
if (!initialized) {
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_v_full_h = param_find("BAT_V_CHARGED");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
initialized = true;
}
if (counter % 100 == 0) {
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_v_load_drop_h, &bat_v_load_drop);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
src/modules/commander/commander_helper.h
+2 -1
View File
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
*
* @param voltage the current battery voltage
* @param discharged the discharged capacity
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage, float discharged);
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+11 -1
View File
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
/**
* Voltage drop per cell on 100% load
*
* This implicitely defines the internal resistance
* to maximum current ratio and assumes linearity.
*
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
/**
* Number of cells.
src/modules/commander/gyro_calibration.cpp
+2 -2
View File
@@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
@@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
src/modules/commander/mag_calibration.cpp
+2 -2
View File
@@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
@@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
src/modules/commander/state_machine_helper.cpp
+20 -10
View File
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
}
}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
}
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
}
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s";
const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
/* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
}
return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
mavlink_log_critical(mavlink_fd, "hold still while arming");
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
/* this is frickin' fatal */
failed = true;
goto system_eval;
@@ -661,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
goto system_eval;
}
if (!status->is_rotary_wing) {
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
src/modules/controllib/block/Block.hpp
+5
View File
@@ -93,6 +93,11 @@ protected:
List<uORB::SubscriptionBase *> _subscriptions;
List<uORB::PublicationBase *> _publications;
List<BlockParamBase *> _params;
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block&);
Block operator=(const control::Block&);
};
class __EXPORT SuperBlock :
src/modules/controllib/blocks.cpp
+15 -1
View File
@@ -121,6 +121,9 @@ int blockLimitSymTest()
float BlockLowPass::update(float input)
{
if (!isfinite(getState())) {
setState(input);
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
@@ -293,7 +296,18 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}
src/modules/controllib/blocks.hpp
+18 -1
View File
@@ -114,7 +114,7 @@ public:
// methods
BlockLowPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_state(0),
_state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockLowPass() {};
@@ -238,9 +238,25 @@ public:
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_initialized(false),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
/**
* Update the state and get current derivative
*
* This call updates the state and gets the current
* derivative. As the derivative is only valid
* on the second call to update, it will return
* no change (0) on the first. To get a closer
* estimate of the derivative on the first call,
* call setU() one time step before using the
* return value of update().
*
* @param input the variable to calculate the derivative of
* @return the current derivative
*/
float update(float input);
// accessors
void setU(float u) {_u = u;}
@@ -249,6 +265,7 @@ public:
protected:
// attributes
float _u; /**< previous input */
bool _initialized;
BlockLowPass _lowPass; /**< low pass filter */
};
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+19 -27
View File
@@ -112,6 +112,10 @@ public:
*/
FixedwingEstimator();
/* we do not want people ever copying this class */
FixedwingEstimator(const FixedwingEstimator& that) = delete;
FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
/**
* Destructor, also kills the sensors task.
*/
@@ -371,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() :
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
_sensor_combined({}),
_sensor_combined{},
#endif
_pos_ref{},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
@@ -390,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_baro_init(false),
_gps_initialized(false),
_gps_start_time(0),
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_ekf_logging(true),
_debug(0),
_mavlink_fd(-1),
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
@@ -619,10 +630,10 @@ FixedwingEstimator::check_filter_state()
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@@ -693,7 +704,7 @@ FixedwingEstimator::task_main()
/*
* do subscriptions
*/
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -1041,7 +1052,7 @@ FixedwingEstimator::task_main()
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude;
@@ -1133,7 +1144,7 @@ FixedwingEstimator::task_main()
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
_baro_gps_offset = _baro.altitude - gps_alt;
_ekf->baroHgt = _baro.altitude;
@@ -1454,25 +1465,6 @@ FixedwingEstimator::task_main()
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+5 -1
View File
@@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
ResetHeight();
ResetStoredStates();
ret = 3;
ret = 0;
}
// Reset the filter if gyro offsets are excessive
@@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
current_ekf_state.states[i] = states[i];
}
current_ekf_state.n_states = n_states;
current_ekf_state.onGround = onGround;
current_ekf_state.staticMode = staticMode;
current_ekf_state.useCompass = useCompass;
current_ekf_state.useAirspeed = useAirspeed;
memcpy(err, &current_ekf_state, sizeof(*err));
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+5 -2
View File
@@ -45,8 +45,11 @@ void Vector3f::zero(void)
z = 0.0f;
}
Mat3f::Mat3f() {
identity();
Mat3f::Mat3f() :
x{1.0f, 0.0f, 0.0f},
y{0.0f, 1.0f, 0.0f},
z{0.0f, 0.0f, 1.0f}
{
}
void Mat3f::identity() {
src/modules/ekf_att_pos_estimator/estimator_utilities.h
+4
View File
@@ -68,6 +68,10 @@ struct ekf_status_report {
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
bool onGround;
bool staticMode;
bool useCompass;
bool useAirspeed;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
src/modules/ekf_att_pos_estimator/module.mk
+2
View File
@@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator_23states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++
src/modules/fw_att_control/fw_att_control_main.cpp
+2 -2
View File
@@ -529,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
}
}
@@ -577,7 +577,7 @@ FixedwingAttitudeControl::task_main()
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+83 -165
View File
@@ -42,8 +42,8 @@
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* Implementation for total energy control class:
* Thomas Gubler
* Original implementation for total energy control class:
* Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
*
* More details and acknowledgements in the referenced library headers.
*
@@ -87,10 +87,13 @@
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
static int _control_task = -1; /**< task handle for sensor task */
/**
* L1 control app start / stop handling function
@@ -115,9 +118,9 @@ public:
/**
* Start the sensors task.
*
* @return OK on success.
* @return OK on success.
*/
int start();
static int start();
/**
* Task status
@@ -131,12 +134,10 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle for sensor task */
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@@ -160,18 +161,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
/** manual control states */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
double _launch_lat;
double _launch_lon;
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
@@ -188,8 +177,8 @@ private:
/* Landingslope object */
Landingslope landingslope;
float flare_curve_alt_rel_last;
/* heading hold */
float target_bearing;
@@ -205,6 +194,7 @@ private:
math::Matrix<3, 3> _R_nb; ///< current attitude
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
@@ -216,6 +206,8 @@ private:
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float heightrate_p;
float speedrate_p;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
@@ -238,9 +230,6 @@ private:
float throttle_land_max;
float heightrate_p;
float speedrate_p;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
@@ -259,6 +248,8 @@ private:
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t heightrate_p;
param_t speedrate_p;
param_t throttle_damp;
param_t integrator_gain;
param_t vertical_accel_limit;
@@ -281,9 +272,6 @@ private:
param_t throttle_land_max;
param_t heightrate_p;
param_t speedrate_p;
param_t land_slope_angle;
param_t land_H1_virt;
param_t land_flare_alt_relative;
@@ -406,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_task_should_exit(false),
_task_running(false),
_control_task(-1),
/* subscriptions */
_global_pos_sub(-1),
@@ -416,12 +403,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
_nav_capabilities_pub(-1),
/* states */
_att(),
_att_sp(),
_nav_capabilities(),
@@ -436,8 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
land_stayonground(false),
@@ -446,12 +433,16 @@ FixedwingPositionControl::FixedwingPositionControl() :
launch_detected(false),
usePreTakeoffThrust(false),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
target_bearing(0.0f),
launchDetector(),
_airspeed_error(0.0f),
_airspeed_valid(false),
_airspeed_last_valid(0),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false),
_l1_control(),
_mTecs(),
_was_pos_control_mode(false)
{
@@ -572,6 +563,23 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
@@ -609,17 +617,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
orb_check(_control_mode_sub, &vstatus_updated);
if (vstatus_updated) {
bool was_armed = _control_mode.flag_armed;
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (!was_armed && _control_mode.flag_armed) {
_launch_lat = _global_pos.lat;
_launch_lon = _global_pos.lon;
_launch_alt = _global_pos.alt;
_launch_valid = true;
}
}
}
@@ -643,6 +641,9 @@ FixedwingPositionControl::vehicle_airspeed_poll()
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
@@ -703,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll()
void
FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
{
l1_control::g_control = new FixedwingPositionControl();
if (l1_control::g_control == nullptr) {
warnx("OUT OF MEM");
return;
}
/* only returns on exit */
l1_control::g_control->task_main();
delete l1_control::g_control;
l1_control::g_control = nullptr;
}
float
@@ -817,7 +828,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
/* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -842,6 +859,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
@@ -949,7 +969,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate altitude of last ordinary waypoint L */
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1115,15 +1135,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
reset_landing_state();
@@ -1139,89 +1150,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
_was_pos_control_mode = false;
@@ -1242,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _mTecs.getPitchSetpoint();
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1260,10 +1188,6 @@ void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
@@ -1339,14 +1263,6 @@ FixedwingPositionControl::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@@ -1432,20 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
limitOverride.disablePitchMinOverride();
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
}
int
@@ -1479,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[])
if (l1_control::g_control != nullptr)
errx(1, "already running");
l1_control::g_control = new FixedwingPositionControl;
if (l1_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != l1_control::g_control->start()) {
delete l1_control::g_control;
l1_control::g_control = nullptr;
if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
}
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+9 -2
View File
@@ -59,6 +59,7 @@ mTecs::mTecs() :
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@@ -75,6 +76,7 @@ mTecs::mTecs() :
_counter(0),
_debug(false)
{
warnx("starting");
}
mTecs::~mTecs()
@@ -93,18 +95,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* time measurement */
updateTimeMeasurement();
/* Filter altitude */
float altitudeFiltered = _altitudeLowpass.update(altitude);
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+1
View File
@@ -115,6 +115,7 @@ protected:
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+3 -3
View File
@@ -72,8 +72,8 @@ public:
* @return: true if the limit is applied, false otherwise
*/
bool limit(float& value, float& difference) {
float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
if (value < minimum) {
difference = value - minimum;
value = minimum;
@@ -86,7 +86,7 @@ public:
return false;
}
//accessor:
bool isAngularLimit() {return _isAngularLimit ;}
bool getIsAngularLimit() {return _isAngularLimit ;}
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
void setMin(float value) { _min.set(value); }
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+16 -9
View File
@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for altitude
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
@@ -234,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
/**
* P gain for the airspeed control
@@ -261,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f);
/**
* Minimal acceleration (air)
@@ -279,13 +293,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
*/
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
/**
* Minimal throttle during takeoff
*
src/modules/mavlink/mavlink_bridge_header.h
+6 -1
View File
@@ -44,7 +44,12 @@
__BEGIN_DECLS
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/*
* We are NOT using convenience functions,
* but instead send messages with a custom function.
* So we do NOT do this:
* #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
*/
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
src/modules/mavlink/mavlink_ftp.cpp
+10 -6
View File
@@ -50,16 +50,20 @@ MavlinkFTP::getServer()
return _server;
}
MavlinkFTP::MavlinkFTP()
MavlinkFTP::MavlinkFTP() :
_session_fds{},
_workBufs{},
_workFree{},
_lock{}
{
// initialise the request freelist
dq_init(&_workFree);
sem_init(&_lock, 0, 1);
// initialize session list
for (size_t i=0; i<kMaxSession; i++) {
_session_fds[i] = -1;
}
// initialize session list
for (size_t i=0; i<kMaxSession; i++) {
_session_fds[i] = -1;
}
// drop work entries onto the free list
for (unsigned i = 0; i < kRequestQueueSize; i++) {
src/modules/mavlink/mavlink_ftp.h
+1
View File
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include "mavlink_messages.h"
#include "mavlink_main.h"
class MavlinkFTP
{
src/modules/mavlink/mavlink_main.cpp
+449 -422
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_main.h
+87 -55
View File
@@ -51,13 +51,14 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
#include "mavlink_bridge_header.h"
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
#include "mavlink_messages.h"
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
class Mavlink
{
@@ -97,6 +98,8 @@ public:
static int destroy_all_instances();
static int get_status_all_instances();
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -157,7 +160,12 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
void send_message(const mavlink_message_t *msg);
void send_message(const uint8_t msgid, const void *msg);
/**
* Resend message as is, don't change sequence number and CRC.
*/
void resend_message(mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
@@ -185,29 +193,33 @@ public:
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_info(const char *string);
void send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_critical(const char *string);
void send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_emergency(const char *string);
void send_statustext_emergency(const char *string);
/**
* Send a status text with loglevel
* Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
* only on this mavlink connection. Useful for reporting communication specific, not system-wide info
* only to client interested in it. Message will be not sent immediately but queued in buffer as
* for mavlink_log_xxx().
*
* @param string the message to send (will be capped by mavlink max string length)
* @param severity the log level, one of
* @param severity the log level
*/
int send_statustext(unsigned severity, const char *string);
void send_statustext(unsigned char severity, const char *string);
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
@@ -229,6 +241,28 @@ public:
*/
void count_txerr();
/**
* Count transmitted bytes
*/
void count_txbytes(unsigned n) { _bytes_tx += n; };
/**
* Count bytes not transmitted because of errors
*/
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
/**
* Count received bytes
*/
void count_rxbytes(unsigned n) { _bytes_rx += n; };
/**
* Get the receive status of this MAVLink link
*/
struct telemetry_status_s& get_rx_status() { return _rstatus; }
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
protected:
Mavlink *next;
@@ -250,13 +284,12 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
MavlinkMissionManager *_mission_manager;
MavlinkMissionManager *_mission_manager;
MavlinkParametersManager *_parameters_manager;
orb_advert_t _mission_pub;
int _mission_result_sub;
MAVLINK_MODE _mode;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
@@ -269,7 +302,9 @@ private:
bool _ftp_on;
int _uart_fd;
int _baudrate;
int _datarate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
/**
* If the queue index is not at 0, the queue sending
@@ -284,6 +319,18 @@ private:
float _subscribe_to_stream_rate;
bool _flow_control_enabled;
uint64_t _last_write_success_time;
uint64_t _last_write_try_time;
unsigned _bytes_tx;
unsigned _bytes_txerr;
unsigned _bytes_rx;
uint64_t _bytes_timestamp;
float _rate_tx;
float _rate_txerr;
float _rate_rx;
struct telemetry_status_s _rstatus; ///< receive status
struct mavlink_message_buffer {
int write_ptr;
@@ -295,6 +342,7 @@ private:
mavlink_message_buffer _message_buffer;
pthread_mutex_t _message_buffer_mutex;
pthread_mutex_t _send_mutex;
bool _param_initialized;
param_t _param_system_id;
@@ -305,53 +353,28 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/**
* Send one parameter.
*
* @param param The parameter id to send.
* @return zero on success, nonzero on failure.
*/
int mavlink_pm_send_param(param_t param);
/**
* Send one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_index(uint16_t index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int mavlink_pm_send_param_for_name(const char *name);
/**
* Send a queue of parameters, one parameter per function call.
*
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
/**
* Get the free space in the transmit buffer
*
* @return free space in the UART TX buffer
*/
unsigned get_free_tx_buf();
static unsigned int interval_from_rate(float rate);
int configure_stream(const char *stream_name, const float rate);
/**
* Adjust the stream rates based on the current rate
*
* @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
*/
void adjust_stream_rates(const float multiplier);
int message_buffer_init(int size);
void message_buffer_destroy();
@@ -366,10 +389,19 @@ private:
void pass_message(const mavlink_message_t *msg);
/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
void update_rate_mult();
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
/**
* Main mavlink task.
*/
int task_main(int argc, char *argv[]);
/* do not allow copying this class */
Mavlink(const Mavlink&);
Mavlink operator=(const Mavlink&);
};
src/modules/mavlink/mavlink_messages.cpp
+917 -549
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_messages.h
+2 -2
View File
@@ -46,10 +46,10 @@
class StreamListItem {
public:
MavlinkStream* (*new_instance)();
MavlinkStream* (*new_instance)(Mavlink *mavlink);
const char* (*get_name)();
StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
new_instance(inst),
get_name(name) {};
src/modules/mavlink/mavlink_mission.cpp
+27 -24
View File
@@ -59,8 +59,7 @@
static const int ERROR = -1;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink),
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_offboard_mission_sub(-1),
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_channel(mavlink->get_channel()),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
close(_mission_result_sub);
}
unsigned
MavlinkMissionManager::get_size()
{
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
void
MavlinkMissionManager::init_offboard_mission()
{
@@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
@@ -175,13 +185,11 @@ void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
if (seq < _count) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
} else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */
@@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
{
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _count;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
@@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mavlink_mission_item_t wp;
format_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
@@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (seq < _max_count) {
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
@@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
void
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
_mavlink->send_message(&msg);
_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
}
void
MavlinkMissionManager::eventloop()
MavlinkMissionManager::send(const hrt_abstime now)
{
hrt_abstime now = hrt_absolute_time();
/* update interval for slow rate limiter */
_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
bool updated = false;
orb_check(_mission_result_sub, &updated);
src/modules/mavlink/mavlink_mission.h
+69 -48
View File
@@ -42,11 +42,12 @@
#pragma once
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include <uORB/uORB.h>
// FIXME XXX - TO BE MOVED TO XML
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include "mavlink_stream.h"
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
@@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES {
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class Mavlink;
class MavlinkMissionManager {
class MavlinkMissionManager : public MavlinkStream {
public:
MavlinkMissionManager(Mavlink *parent);
~MavlinkMissionManager();
const char *get_name() const
{
return MavlinkMissionManager::get_name_static();
}
static const char *get_name_static()
{
return "MISSION_ITEM";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_MISSION_ITEM;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkMissionManager(mavlink);
}
unsigned get_size();
void handle_message(const mavlink_message_t *msg);
void set_verbose(bool v) { _verbose = v; }
private:
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
void init_offboard_mission();
int update_active_mission(int dataman_id, unsigned count, int seq);
void send_message(mavlink_message_t *msg);
/**
* @brief Sends an waypoint ack message
*/
@@ -111,10 +167,6 @@ public:
*/
void send_mission_item_reached(uint16_t seq);
void eventloop();
void handle_message(const mavlink_message_t *msg);
void handle_mission_ack(const mavlink_message_t *msg);
void handle_mission_set_current(const mavlink_message_t *msg);
@@ -139,39 +191,8 @@ public:
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void set_verbose(bool v) { _verbose = v; }
protected:
explicit MavlinkMissionManager(Mavlink *mavlink);
private:
Mavlink* _mavlink;
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
mavlink_channel_t _channel;
uint8_t _comp_id;
void send(const hrt_abstime t);
};
src/modules/mavlink/mavlink_orb_subscription.h
+4
View File
@@ -82,6 +82,10 @@ private:
const orb_id_t _topic; ///< topic metadata
int _fd; ///< subscription handle
bool _published; ///< topic was ever published
/* do not allow copying this class */
MavlinkOrbSubscription(const MavlinkOrbSubscription&);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&);
};
src/modules/mavlink/mavlink_parameters.cpp
+197
View File
@@ -0,0 +1,197 @@
/****************************************************************************
*
* 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 mavlink_parameters.cpp
* Mavlink parameters manager implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
#include "mavlink_parameters.h"
#include "mavlink_main.h"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1)
{
}
unsigned
MavlinkParametersManager::get_size()
{
if (_send_all_index >= 0) {
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
}
void
MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* request all parameters */
mavlink_param_request_list_t req_list;
mavlink_msg_param_request_list_decode(msg, &req_list);
if (req_list.target_system == mavlink_system.sysid &&
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
_send_all_index = 0;
_mavlink->send_statustext_info("[pm] sending list");
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */
param_t param = param_find(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(set.param_value));
send_param(param);
}
}
}
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
/* request one parameter */
mavlink_param_request_read_t req_read;
mavlink_msg_param_request_read_decode(msg, &req_read);
if (req_read.target_system == mavlink_system.sysid &&
(req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find(name));
} else {
/* when index is >= 0, send this parameter again */
send_param(param_for_index(req_read.param_index));
}
}
break;
}
default:
break;
}
}
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested */
if (_send_all_index >= 0) {
send_param(param_for_index(_send_all_index));
_send_all_index++;
if (_send_all_index >= (int) param_count()) {
_send_all_index = -1;
}
}
}
void
MavlinkParametersManager::send_param(param_t param)
{
if (param == PARAM_INVALID) {
return;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return;
}
msg.param_count = param_count();
msg.param_index = param_get_index(param);
/* copy parameter name */
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
}
src/modules/mavlink/mavlink_parameters.h
+115
View File
@@ -0,0 +1,115 @@
/****************************************************************************
*
* 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 mavlink_parameters.h
* Mavlink parameters manager definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include <systemlib/param/param.h>
#include "mavlink_bridge_header.h"
#include "mavlink_stream.h"
class MavlinkParametersManager : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkParametersManager::get_name_static();
}
static const char *get_name_static()
{
return "PARAM_VALUE";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_PARAM_VALUE;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkParametersManager(mavlink);
}
unsigned get_size();
void handle_message(const mavlink_message_t *msg);
/**
* Send one parameter identified by index.
*
* @param index The index of the parameter to send.
* @return zero on success, nonzero else.
*/
void start_send_one(int index);
/**
* Send one parameter identified by name.
*
* @param name The index of the parameter to send.
* @return zero on success, nonzero else.
*/
int start_send_for_name(const char *name);
/**
* Start sending the parameter queue.
*
* This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of
* mavlink_pm_queued_send().
* @see mavlink_pm_queued_send()
*/
void start_send_all();
private:
int _send_all_index;
/* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &);
MavlinkParametersManager& operator = (const MavlinkParametersManager &);
protected:
explicit MavlinkParametersManager(Mavlink *mavlink);
void send(const hrt_abstime t);
void send_param(param_t param);
};
src/modules/mavlink/mavlink_rate_limiter.cpp
+1 -1
View File
@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval;
_last_sent = t;
return true;
}
src/modules/mavlink/mavlink_receiver.cpp
+34 -27
View File
@@ -86,7 +86,9 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
status{},
hil_local_pos{},
_control_mode{},
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -110,17 +112,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_control_mode_sub(-1),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
_hil_local_alt0(0.0)
_hil_local_alt0(0.0f),
_hil_local_proj_ref{}
{
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
memset(&hil_local_pos, 0, sizeof(hil_local_pos));
memset(&_control_mode, 0, sizeof(_control_mode));
// make sure the FTP server is started
(void)MavlinkFTP::getServer();
@@ -247,7 +246,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
@@ -444,11 +444,11 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
@@ -505,16 +505,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.timestamp = tnow;
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
@@ -590,10 +594,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
gyro.temperature = imu.temperature;
if (_gyro_pub < 0) {
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
}
}
@@ -612,10 +616,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
accel.temperature = imu.temperature;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
}
}
@@ -633,10 +637,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
mag.z = imu.zmag;
if (_mag_pub < 0) {
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
}
}
@@ -651,10 +655,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
baro.temperature = imu.temperature;
if (_baro_pub < 0) {
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
} else {
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
}
}
@@ -928,10 +932,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
accel.temperature = 25.0f;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
}
}
@@ -999,6 +1003,9 @@ MavlinkReceiver::receive_thread(void *arg)
_mavlink->handle_message(&msg);
}
}
/* count received bytes */
_mavlink->count_rxbytes(nread);
}
}
src/modules/mavlink/mavlink_receiver.h
+4 -1
View File
@@ -151,7 +151,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
@@ -159,4 +158,8 @@ private:
bool _hil_local_proj_inited;
float _hil_local_alt0;
struct map_projection_reference_s _hil_local_proj_ref;
/* do not allow copying this class */
MavlinkReceiver(const MavlinkReceiver&);
MavlinkReceiver operator=(const MavlinkReceiver&);
};
src/modules/mavlink/mavlink_stream.cpp
+14 -13
View File
@@ -43,9 +43,9 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() :
MavlinkStream::MavlinkStream(Mavlink *mavlink) :
next(nullptr),
_channel(MAVLINK_COMM_0),
_mavlink(mavlink),
_interval(1000000),
_last_sent(0)
{
@@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval)
_interval = interval;
}
/**
* Set mavlink channel
*/
void
MavlinkStream::set_channel(mavlink_channel_t channel)
{
_channel = channel;
}
/**
* Update subscriptions and send message if necessary
*/
@@ -80,11 +71,21 @@ int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
unsigned int interval = _interval;
if (dt > 0 && dt >= _interval) {
if (!const_rate()) {
interval /= _mavlink->get_rate_mult();
}
if (dt > 0 && dt >= interval) {
/* interval expired, send message */
send(t);
_last_sent = (t / _interval) * _interval;
if (const_rate()) {
_last_sent = (t / _interval) * _interval;
} else {
_last_sent = t;
}
return 0;
}
src/modules/mavlink/mavlink_stream.h
+30 -7
View File
@@ -46,37 +46,60 @@
class Mavlink;
class MavlinkStream;
#include "mavlink_main.h"
class MavlinkStream
{
public:
MavlinkStream *next;
MavlinkStream();
MavlinkStream(Mavlink *mavlink);
virtual ~MavlinkStream();
/**
* Get the interval
*
* @param interval the inveral in microseconds (us) between messages
*/
void set_interval(const unsigned int interval);
void set_channel(mavlink_channel_t channel);
/**
* Get the interval
*
* @return the inveral in microseconds (us) between messages
*/
unsigned get_interval() { return _interval; }
/**
* @return 0 if updated / sent, -1 if unchanged
*/
int update(const hrt_abstime t);
static MavlinkStream *new_instance();
static MavlinkStream *new_instance(const Mavlink *mavlink);
static const char *get_name_static();
virtual void subscribe(Mavlink *mavlink) = 0;
virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
/**
* @return true if steam rate shouldn't be adjusted
*/
virtual bool const_rate() { return false; }
/**
* Get maximal total messages size on update
*/
virtual unsigned get_size() = 0;
protected:
mavlink_channel_t _channel;
Mavlink * _mavlink;
unsigned int _interval;
virtual void send(const hrt_abstime t) = 0;
private:
hrt_abstime _last_sent;
/* do not allow top copying this class */
MavlinkStream(const MavlinkStream&);
MavlinkStream& operator=(const MavlinkStream&);
};
src/modules/mavlink/module.mk
+3 -1
View File
@@ -40,11 +40,11 @@ SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
mavlink_parameters.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \
mavlink_rate_limiter.cpp \
mavlink_commands.cpp \
mavlink_ftp.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
@@ -52,3 +52,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1024
EXTRACXXFLAGS = -Weffc++
src/modules/navigator/mission.cpp
+21 -15
View File
@@ -314,30 +314,36 @@ Mission::set_mission_items()
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = !home_dist_ok;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available, switch to loiter */
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: no mission available");
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
*/
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
}
_mission_type = MISSION_TYPE_NONE;
@@ -397,7 +403,7 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
@@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
"ERROR waypoint could not be read");
return false;
}
@@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP waypoint could not be written");
"ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
mavlink_log_critical(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP is cycling, giving up");
"ERROR DO JUMP is cycling, giving up");
return false;
}
src/modules/navigator/mission_feasibility_checker.cpp
+6 -3
View File
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
if (home_alt > missionitem.altitude) {
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if (home_alt > wp_alt) {
if (throw_error) {
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
} else {
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
return true;
}
}
src/modules/navigator/module.mk
+2
View File
@@ -54,3 +54,5 @@ SRCS = navigator_main.cpp \
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
src/modules/navigator/navigator.h
+6
View File
@@ -222,5 +222,11 @@ private:
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/
Navigator(const Navigator&);
Navigator operator=(const Navigator&);
};
#endif
src/modules/navigator/navigator_main.cpp
+12 -9
View File
@@ -105,17 +105,18 @@ Navigator::Navigator() :
_control_mode_sub(-1),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_vstatus({}),
_control_mode({}),
_global_pos({}),
_home_pos({}),
_mission_item({}),
_nav_caps({}),
_pos_sp_triplet({}),
_vstatus{},
_control_mode{},
_global_pos{},
_home_pos{},
_mission_item{},
_nav_caps{},
_pos_sp_triplet{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence({}),
_geofence{},
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
@@ -124,6 +125,8 @@ Navigator::Navigator() :
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_offboard(this, "OFF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD")
{
@@ -455,7 +458,7 @@ void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
/* TODO: set nav_state */
_pos_sp_triplet.nav_state = _vstatus.nav_state;
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
src/modules/navigator/navigator_mode.h
+6
View File
@@ -88,6 +88,12 @@ protected:
private:
bool _first_run;
/* this class has ptr data members, so it should not be copied,
* consequently the copy constructors are private.
*/
NavigatorMode(const NavigatorMode&);
NavigatorMode operator=(const NavigatorMode&);
};
#endif
src/modules/position_estimator_inav/position_estimator_inav_params.c
+215 -1
View File
@@ -41,28 +41,242 @@
#include "position_estimator_inav_params.h"
/**
* Z axis weight for barometer
*
* Weight (cutoff frequency) for barometer altitude measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
/**
* Z axis weight for GPS
*
* Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
/**
* Z axis weight for vision
*
* Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
/**
* Z axis weight for sonar
*
* Weight (cutoff frequency) for sonar measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
/**
* XY axis weight for GPS position
*
* Weight (cutoff frequency) for GPS position measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
/**
* XY axis weight for GPS velocity
*
* Weight (cutoff frequency) for GPS velocity measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
/**
* XY axis weight for vision position
*
* Weight (cutoff frequency) for vision position measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
/**
* XY axis weight for vision velocity
*
* Weight (cutoff frequency) for vision velocity measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
/**
* XY axis weight for optical flow
*
* Weight (cutoff frequency) for optical flow (velocity) measurements.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
/**
* XY axis weight for resetting velocity
*
* When velocity sources lost slowly decrease estimated horizontal velocity with this weight.
*
* @min 0.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
/**
* XY axis weight factor for GPS when optical flow available
*
* When optical flow data available, multiply GPS weights (for position and velocity) by this factor.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
/**
* Accelerometer bias estimation weight
*
* Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
*
* @min 0.0
* @max 0.1
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
/**
* Optical flow scale factor
*
* Factor to convert raw optical flow (in pixels) to radians [rad/px].
*
* @min 0.0
* @max 1.0
* @unit rad/px
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
/**
* Minimal acceptable optical flow quality
*
* 0 - lowest quality, 1 - best quality.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
/**
* Weight for sonar filter
*
* Sonar filter detects spikes on sonar measurements and used to detect new surface level.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
/**
* Sonar maximal error for new surface
*
* If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).
*
* @min 0.0
* @max 1.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
/**
* Land detector time
*
* Vehicle assumed landed if no altitude changes happened during this time on low throttle.
*
* @min 0.0
* @max 10.0
* @unit s
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
/**
* Land detector altitude dispersion threshold
*
* Dispersion threshold for triggering land detector.
*
* @min 0.0
* @max 10.0
* @unit m
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
/**
* Land detector throttle threshold
*
* Value should be lower than minimal hovering thrust. Half of it is good choice.
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
/**
* GPS delay
*
* GPS delay compensation
*
* @min 0.0
* @max 1.0
* @unit s
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
/**
* Disable vision input
*
* Set to the appropriate key to disable input
*
* @min 0.0
* @max 1.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_z_baro = param_find("INAV_W_Z_BARO");
src/modules/px4iofirmware/mixer.cpp
+8 -1
View File
@@ -111,7 +111,7 @@ mixer_tick(void)
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
/* default to failsafe mixing */
/* default to failsafe mixing - it will be forced below if flag is set */
source = MIX_FAILSAFE;
/*
@@ -154,6 +154,13 @@ mixer_tick(void)
}
}
/*
* Check if we should force failsafe - and do it if we have to
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
source = MIX_FAILSAFE;
}
/*
* Set failsafe status flag depending on mixing source
*/
src/modules/px4iofirmware/protocol.h
+5
View File
@@ -179,6 +179,7 @@
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@@ -253,6 +254,10 @@ enum { /* DSM bind states */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */
#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
src/modules/px4iofirmware/registers.c
+9 -19
View File
@@ -189,7 +189,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN)
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -738,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
* range on one unit, validated on sample points of another unit
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
* Data in Tools/tests-host/data folder.
*
* slope = 0.0046067
* intercept = 0.3863
* measured slope = 0.004585267878277 (int: 4585)
* nominal theoretic slope: 0.00459340659 (int: 4593)
* intercept = 0.016646394188076 (int: 16646)
* nominal theoretic intercept: 0.00 (int: 0)
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
if (counts != 0xffff) {
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned mV = (166460 + (counts * 45934)) / 10000;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
src/modules/sdlog2/sdlog2.c
+81 -11
View File
@@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
hrt_abstime gyro1_timestamp = 0;
hrt_abstime accelerometer1_timestamp = 0;
hrt_abstime magnetometer1_timestamp = 0;
hrt_abstime gyro2_timestamp = 0;
hrt_abstime accelerometer2_timestamp = 0;
hrt_abstime magnetometer2_timestamp = 0;
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
@@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
bool write_SENS = false;
if (buf.sensor.timestamp != gyro_timestamp) {
@@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
gyro1_timestamp = buf.sensor.gyro1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
write_IMU1 = true;
}
if (write_IMU1) {
log_msg.msg_type = LOG_IMU1_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
gyro2_timestamp = buf.sensor.gyro2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
write_IMU2 = true;
}
if (write_IMU2) {
log_msg.msg_type = LOG_IMU2_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
}
/* --- ATTITUDE --- */
@@ -1366,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
if (buf.triplet.current.valid) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
}
/* --- VICON POSITION --- */
@@ -1529,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
src/modules/sdlog2/sdlog2_messages.h
+9 -4
View File
@@ -73,6 +73,8 @@ struct log_ATSP_s {
/* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4
#define LOG_IMU1_MSG 22
#define LOG_IMU2_MSG 23
struct log_IMU_s {
float acc_x;
float acc_y;
@@ -276,8 +278,8 @@ struct log_DIST_s {
uint8_t flags;
};
// ID 22 available
// ID 23 available
/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
@@ -332,6 +334,7 @@ struct log_GS1B_s {
struct log_TECS_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
@@ -420,7 +423,9 @@ static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
@@ -450,7 +455,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
src/modules/sensors/sensor_params.c
+13
View File
@@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
*/
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* Set usage of external magnetometer
*
* * Set to 0 (default) to auto-detect (will try to get the external as primary)
* * Set to 1 to force the external magnetometer as primary
* * Set to 2 to force the internal magnetometer as primary
*
* @min 0
* @max 2
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_EXT_MAG, 0);
/**
* RC Channel 1 Minimum
src/modules/sensors/sensors.cpp
+115 -11
View File
@@ -199,9 +199,15 @@ private:
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
int _gyro_sub; /**< raw gyro data subscription */
int _accel_sub; /**< raw accel data subscription */
int _mag_sub; /**< raw mag data subscription */
int _gyro_sub; /**< raw gyro0 data subscription */
int _accel_sub; /**< raw accel0 data subscription */
int _mag_sub; /**< raw mag0 data subscription */
int _gyro1_sub; /**< raw gyro1 data subscription */
int _accel1_sub; /**< raw accel1 data subscription */
int _mag1_sub; /**< raw mag1 data subscription */
int _gyro2_sub; /**< raw gyro2 data subscription */
int _accel2_sub; /**< raw accel2 data subscription */
int _mag2_sub; /**< raw mag2 data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */
@@ -478,6 +484,12 @@ Sensors::Sensors() :
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
_gyro1_sub(-1),
_accel1_sub(-1),
_mag1_sub(-1),
_gyro2_sub(-1),
_accel2_sub(-1),
_mag2_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
_vcontrol_mode_sub(-1),
@@ -1004,7 +1016,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
@@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
raw.accelerometer_timestamp = accel_report.timestamp;
}
orb_check(_accel1_sub, &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer1_m_s2[0] = vect(0);
raw.accelerometer1_m_s2[1] = vect(1);
raw.accelerometer1_m_s2[2] = vect(2);
raw.accelerometer1_raw[0] = accel_report.x_raw;
raw.accelerometer1_raw[1] = accel_report.y_raw;
raw.accelerometer1_raw[2] = accel_report.z_raw;
raw.accelerometer1_timestamp = accel_report.timestamp;
}
orb_check(_accel2_sub, &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer2_m_s2[0] = vect(0);
raw.accelerometer2_m_s2[1] = vect(1);
raw.accelerometer2_m_s2[2] = vect(2);
raw.accelerometer2_raw[0] = accel_report.x_raw;
raw.accelerometer2_raw[1] = accel_report.y_raw;
raw.accelerometer2_raw[2] = accel_report.z_raw;
raw.accelerometer2_timestamp = accel_report.timestamp;
}
}
void
@@ -1030,7 +1084,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
@@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.timestamp = gyro_report.timestamp;
}
orb_check(_gyro1_sub, &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro1_rad_s[0] = vect(0);
raw.gyro1_rad_s[1] = vect(1);
raw.gyro1_rad_s[2] = vect(2);
raw.gyro1_raw[0] = gyro_report.x_raw;
raw.gyro1_raw[1] = gyro_report.y_raw;
raw.gyro1_raw[2] = gyro_report.z_raw;
raw.gyro1_timestamp = gyro_report.timestamp;
}
orb_check(_gyro2_sub, &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro2_rad_s[0] = vect(0);
raw.gyro2_rad_s[1] = vect(1);
raw.gyro2_rad_s[2] = vect(2);
raw.gyro2_raw[0] = gyro_report.x_raw;
raw.gyro2_raw[1] = gyro_report.y_raw;
raw.gyro2_raw[2] = gyro_report.z_raw;
raw.gyro2_timestamp = gyro_report.timestamp;
}
}
void
@@ -1056,10 +1152,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
// XXX we need device-id based handling here
if (_mag_is_external) {
vect = _external_mag_rotation * vect;
@@ -1087,7 +1185,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@@ -1618,11 +1716,17 @@ Sensors::task_main()
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
src/modules/systemlib/circuit_breaker.c
+12
View File
@@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
/**
* Circuit breaker for airspeed sensor
*
* Setting this parameter to 162128 will disable the check for an airspeed sensor.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 162128
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
src/modules/systemlib/circuit_breaker.h
+1
View File
@@ -52,6 +52,7 @@
#define CBRK_SUPPLY_CHK_KEY 894281
#define CBRK_RATE_CTRL_KEY 140253
#define CBRK_IO_SAFETY_KEY 22027
#define CBRK_AIRSPD_CHK_KEY 162128
#include <stdbool.h>
src/modules/systemlib/mixer/mixer.h
+15
View File
@@ -231,6 +231,10 @@ protected:
static const char * skipline(const char *buf, unsigned &buflen);
private:
/* do not allow to copy due to prt data members */
Mixer(const Mixer&);
Mixer& operator=(const Mixer&);
};
/**
@@ -307,6 +311,10 @@ public:
private:
Mixer *_first; /**< linked list of mixers */
/* do not allow to copy due to pointer data members */
MixerGroup(const MixerGroup&);
MixerGroup operator=(const MixerGroup&);
};
/**
@@ -424,6 +432,10 @@ private:
mixer_scaler_s &scaler,
uint8_t &control_group,
uint8_t &control_index);
/* do not allow to copy due to ptr data members */
SimpleMixer(const SimpleMixer&);
SimpleMixer operator=(const SimpleMixer&);
};
/**
@@ -522,6 +534,9 @@ private:
unsigned _rotor_count;
const Rotor *_rotors;
/* do not allow to copy due to ptr data members */
MultirotorMixer(const MultirotorMixer&);
MultirotorMixer operator=(const MultirotorMixer&);
};
#endif
src/modules/uORB/objects_common.cpp
+11 -4
View File
@@ -46,16 +46,23 @@
#include <drivers/drv_orb_dev.h>
#include <drivers/drv_mag.h>
ORB_DEFINE(sensor_mag, struct mag_report);
ORB_DEFINE(sensor_mag0, struct mag_report);
ORB_DEFINE(sensor_mag1, struct mag_report);
ORB_DEFINE(sensor_mag2, struct mag_report);
#include <drivers/drv_accel.h>
ORB_DEFINE(sensor_accel, struct accel_report);
ORB_DEFINE(sensor_accel0, struct accel_report);
ORB_DEFINE(sensor_accel1, struct accel_report);
ORB_DEFINE(sensor_accel2, struct accel_report);
#include <drivers/drv_gyro.h>
ORB_DEFINE(sensor_gyro, struct gyro_report);
ORB_DEFINE(sensor_gyro0, struct gyro_report);
ORB_DEFINE(sensor_gyro1, struct gyro_report);
ORB_DEFINE(sensor_gyro2, struct gyro_report);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
ORB_DEFINE(sensor_baro0, struct baro_report);
ORB_DEFINE(sensor_baro1, struct baro_report);
#include <drivers/drv_range_finder.h>
ORB_DEFINE(sensor_range_finder, struct range_finder_report);
src/modules/uORB/topics/actuator_armed.h
+2 -1
View File
@@ -56,6 +56,7 @@ struct actuator_armed_s {
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 */
};
/**
@@ -65,4 +66,4 @@ struct actuator_armed_s {
/* register this as object request broker structure */
ORB_DECLARE(actuator_armed);
#endif
#endif
src/modules/uORB/topics/position_setpoint_triplet.h
+2
View File
@@ -95,6 +95,8 @@ 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 */
};
/**
src/modules/uORB/topics/sensor_combined.h
+24
View File
@@ -95,6 +95,30 @@ struct sensor_combined_s {
float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */
uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */
int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */
float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */
uint64_t gyro1_timestamp; /**< Gyro timestamp */
int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */
float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */
int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */
int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */
float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */
uint64_t gyro2_timestamp; /**< Gyro timestamp */
int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */
float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */
int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */
src/modules/uORB/topics/tecs_status.h
+1
View File
@@ -66,6 +66,7 @@ struct tecs_status_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
src/modules/uORB/topics/telemetry_status.h
+2 -1
View File
@@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
struct telemetry_status_s {
uint64_t timestamp;
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */
src/modules/uORB/topics/vehicle_command.h
+1
View File
@@ -76,6 +76,7 @@ enum VEHICLE_CMD {
VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */
VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */
VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */
src/modules/uORB/topics/vehicle_global_position.h
+2 -2
View File
@@ -70,8 +70,8 @@ struct vehicle_global_position_s {
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float eph;
float epv;
float eph; /**< Standard deviation of position estimate horizontally */
float epv; /**< Standard deviation of position vertically */
};
/**
src/modules/uORB/topics/vehicle_status.h
+2 -1
View File
@@ -186,7 +186,7 @@ struct vehicle_status_s {
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 gps signal is good enough to use it in the position estimator */
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;
@@ -226,6 +226,7 @@ struct vehicle_status_s {
uint16_t errors_count4;
bool circuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
};
/**
src/modules/uavcan/.gitignore
+1
View File
@@ -0,0 +1 @@
./dsdlc_generated/
src/modules/uavcan/esc_controller.cpp
+138
View File
@@ -0,0 +1,138 @@
/****************************************************************************
*
* 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 esc_controller.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
_uavcan_sub_status(node),
_orb_timer(node)
{
}
UavcanEscController::~UavcanEscController()
{
perf_free(_perfcnt_invalid_input);
perf_free(_perfcnt_scaling_error);
}
int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
warnx("ESC status sub failed %i", res);
return res;
}
// ESC status will be relayed from UAVCAN bus into ORB at this rate
_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
return res;
}
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
perf_count(_perfcnt_invalid_input);
return;
}
/*
* Rate limiting - we don't want to congest the bus
*/
const auto timestamp = _node.getMonotonicTime();
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
return;
}
_prev_cmd_pub = timestamp;
/*
* Fill the command message
* If unarmed, we publish an empty message anyway
*/
uavcan::equipment::esc::RawCommand msg;
if (_armed) {
for (unsigned i = 0; i < num_outputs; i++) {
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
}
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
perf_count(_perfcnt_scaling_error);
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
perf_count(_perfcnt_scaling_error);
} else {
; // Correct value
}
msg.cmd.push_back(static_cast<unsigned>(scaled));
}
}
/*
* Publish the command message to the bus
* Note that for a quadrotor it takes one CAN frame
*/
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
void UavcanEscController::arm_esc(bool arm)
{
_armed = arm;
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
{
// TODO publish to ORB
}
src/modules/uavcan/esc_controller.hpp
+107
View File
@@ -0,0 +1,107 @@
/****************************************************************************
*
* 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 esc_controller.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand
* uavcan.equipment.esc.RPMCommand
* uavcan.equipment.esc.Status
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <systemlib/perf_counter.h>
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
~UavcanEscController();
int init();
void update_outputs(float *outputs, unsigned num_outputs);
void arm_esc(bool arm);
private:
/**
* ESC status message reception will be reported via this callback.
*/
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
/**
* ESC status will be published to ORB from this callback (fixed rate).
*/
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
typedef uavcan::MethodBinder<UavcanEscController*,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
/*
* libuavcan related things
*/
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
bool _armed = false;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters
*/
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
};
src/modules/uavcan/gnss_receiver.cpp
+153
View File
@@ -0,0 +1,153 @@
/****************************************************************************
*
* 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 gnss_receiver.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
return res;
}
// Clear the uORB GPS report
memset(&_report, 0, sizeof(_report));
return res;
}
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
_report.timestamp_position = hrt_absolute_time();
_report.lat = msg.lat_1e7;
_report.lon = msg.lon_1e7;
_report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
_report.timestamp_variance = _report.timestamp_position;
// Check if the msg contains valid covariance information
const bool valid_position_covariance = !msg.position_covariance.empty();
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
if (valid_position_covariance) {
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
_report.eph = -1.0F;
_report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
*
* Nonlinear equation:
* heading = atan2(vel_e_m_s, vel_n_m_s)
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
*
* To calculate the variance of heading from the variance of velocity,
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
*/
float vel_n = msg.ned_velocity[0];
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
_report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
_report.s_variance_m_s = -1.0F;
_report.c_variance_rad = -1.0F;
}
_report.fix_type = msg.status;
_report.timestamp_velocity = _report.timestamp_position;
_report.vel_n_m_s = msg.ned_velocity[0];
_report.vel_e_m_s = msg.ned_velocity[1];
_report.vel_d_m_s = msg.ned_velocity[2];
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
_report.vel_ned_valid = true;
_report.timestamp_time = _report.timestamp_position;
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
_report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}
src/modules/mavlink/mavlink_commands.cpp → src/modules/uavcan/gnss_receiver.hpp
+44 -30
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 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,39 +32,53 @@
****************************************************************************/
/**
* @file mavlink_commands.cpp
* Mavlink commands stream implementation.
* @file gnss_receiver.hpp
*
* @author Anton Babushkin <anton.babushkin@me.com>
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*/
#include "mavlink_commands.h"
#pragma once
MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
{
_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
}
public:
UavcanGnssReceiver(uavcan::INode& node);
void
MavlinkCommandsStream::update(const hrt_abstime t)
{
struct vehicle_command_s cmd;
int init();
if (_cmd_sub->update(&_cmd_time, &cmd)) {
/* only send commands for other systems/components */
if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
mavlink_msg_command_long_send(_channel,
cmd.target_system,
cmd.target_component,
cmd.command,
cmd.confirmation,
cmd.param1,
cmd.param2,
cmd.param3,
cmd.param4,
cmd.param5,
cmd.param6,
cmd.param7);
}
}
}
private:
/**
* GNSS fix message will be reported via this callback.
*/
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
/*
* uORB
*/
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
};
src/modules/uavcan/module.mk
+74
View File
@@ -0,0 +1,74 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
# Author: Pavel Kirienko <pavel.kirienko@gmail.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, 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.
#
############################################################################
#
# UAVCAN <--> uORB bridge
#
MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
#
# libuavcan
#
include $(UAVCAN_DIR)/libuavcan/include.mk
SRCS += $(LIBUAVCAN_SRC)
INCLUDE_DIRS += $(LIBUAVCAN_INC)
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
#
# libuavcan drivers for STM32
#
include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
SRCS += $(LIBUAVCAN_STM32_SRC)
INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
#
# Invoke DSDL compiler
# TODO: Add make target for this, or invoke dsdlc manually.
# The second option assumes that the generated headers shall be saved
# under the version control, which may be undesirable.
# The first option requires any Python and the Python Mako library for the sources to be built.
#
$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
INCLUDE_DIRS += dsdlc_generated
src/modules/mavlink/mavlink_commands.h → src/modules/uavcan/uavcan_clock.cpp
+39 -25
View File
@@ -31,35 +31,49 @@
*
****************************************************************************/
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/drv_hrt.h>
/**
* @file mavlink_commands.h
* Mavlink commands stream definition.
* @file uavcan_clock.cpp
*
* @author Anton Babushkin <anton.babushkin@me.com>
* Implements a clock for the CAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef MAVLINK_COMMANDS_H_
#define MAVLINK_COMMANDS_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
class Mavlink;
class MavlinkCommansStream;
#include "mavlink_main.h"
class MavlinkCommandsStream
namespace uavcan_stm32
{
namespace clock
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
void update(const hrt_abstime t);
};
uavcan::MonotonicTime getMonotonic()
{
return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
}
uavcan::UtcTime getUtc()
{
return uavcan::UtcTime();
}
void adjustUtc(uavcan::UtcDuration adjustment)
{
(void)adjustment;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return 0;
}
} // namespace clock
SystemClock &SystemClock::instance()
{
static SystemClock inst;
return inst;
}
}
#endif /* MAVLINK_COMMANDS_H_ */
src/modules/uavcan/uavcan_main.cpp
+582
View File
@@ -0,0 +1,582 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#include <nuttx/config.h>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
/**
* @file uavcan_main.cpp
*
* Implements basic functinality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
/*
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_esc_controller(_node),
_gnss_receiver(_node)
{
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
// memset(_controls, 0, sizeof(_controls));
// memset(_poll_fds, 0, sizeof(_poll_fds));
}
UavcanNode::~UavcanNode()
{
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 10;
do {
/* wait 5ms - it should wake every 10ms or so worst-case */
usleep(5000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
_instance = nullptr;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* GPIO config.
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
* fail during initialization.
*/
stm32_configgpio(GPIO_CAN1_RX);
stm32_configgpio(GPIO_CAN1_TX);
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
stm32_configgpio(GPIO_CAN2_TX);
/*
* CAN driver init
*/
static CanInitHelper can;
static bool can_initialized = false;
if (!can_initialized) {
const int can_init_res = can.init(bitrate);
if (can_init_res < 0) {
warnx("CAN driver init failed %i", can_init_res);
return can_init_res;
}
can_initialized = true;
}
/*
* Node init
*/
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
delete _instance;
_instance = nullptr;
warnx("Node init failed %i", node_init_res);
return node_init_res;
}
/*
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
warnx("start failed: %d", errno);
return -errno;
}
return OK;
}
int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
ret = _esc_controller.init();
if (ret < 0)
return ret;
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
uavcan::protocol::SoftwareVersion swver;
swver.major = 12; // TODO fill version info
swver.minor = 34;
_node.setSoftwareVersion(swver);
uavcan::protocol::HardwareVersion hwver;
hwver.major = 42; // TODO fill version info
hwver.minor = 42;
_node.setHardwareVersion(hwver);
_node.setName("org.pixhawk"); // Huh?
_node.setNodeID(node_id);
return _node.start();
}
void UavcanNode::node_spin_once()
{
const int spin_res = _node.spin(uavcan::MonotonicTime());
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
int UavcanNode::run()
{
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0)
{
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true;
}
/*
* XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
node_spin_once(); // Non-blocking
// this would be bad...
if (poll_ret < 0) {
log("poll error %d", errno);
continue;
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
controls_updated = true;
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
if (!controls_updated) {
// timeout: no control data, switch to failsafe values
// XXX trigger failsafe
}
//can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
unsigned num_outputs_max = 8;
// Do mixing
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
outputs.timestamp = hrt_absolute_time();
// iterate actuators
for (unsigned i = 0; i < outputs.noutputs; i++) {
// last resort: catch NaN, INF and out-of-band errors
if (!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
// limit outputs to valid range
// never go below min
if (outputs.output[i] < -1.0f) {
outputs.output[i] = -1.0f;
}
// never go below max
if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
}
}
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
// Update the armed status and check that we're not locked down
bool set_armed = _armed.armed && !_armed.lockdown;
arm_actuators(set_armed);
}
}
teardown();
warnx("exiting.");
exit(0);
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
return 0;
}
int
UavcanNode::teardown()
{
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
return ::close(_armed_sub);
}
int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
_esc_controller.arm_esc(arm);
return OK;
}
void
UavcanNode::subscribe()
{
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < 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]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
int
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
arm_actuators(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
arm_actuators(false);
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _output_count;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
warnx("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
unlock();
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
void
UavcanNode::print_info()
{
if (!_instance) {
warnx("not running, start first");
}
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
}
/*
* App entry point
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
{
constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
if (argc < 3) {
print_usage();
::exit(1);
}
/*
* Node ID
*/
const int node_id = atoi(argv[2]);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
/*
* CAN bitrate
*/
unsigned bitrate = 0;
if (argc > 3) {
bitrate = atol(argv[3]);
}
if (bitrate <= 0) {
bitrate = DEFAULT_CAN_BITRATE;
}
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Start
*/
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
return OK;
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
}
print_usage();
::exit(1);
}
src/modules/uavcan/uavcan_main.hpp
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/device/device.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
/**
* @file uavcan_main.hpp
*
* Defines basic functinality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
/**
* A UAVCAN node.
*/
class UavcanNode : public device::CDev
{
static constexpr unsigned MemPoolSize = 10752;
static constexpr unsigned RxQueueLenPerIface = 64;
static constexpr unsigned StackSize = 3000;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
virtual ~UavcanNode();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
void subscribe();
int teardown();
int arm_actuators(bool arm);
void print_info();
static UavcanNode* instance() { return _instance; }
private:
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
int _armed_sub = -1; ///< uORB subscription of the arming status
actuator_armed_s _armed; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
MixerGroup *_mixers = nullptr;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
unsigned _poll_fds_num = 0;
};