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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-28 23:05:58 +02:00
parent c0dc9f99a6 700594e743
commit 361f858c14
137 changed files with 3716 additions and 2380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+44 -11
View File
@@ -108,6 +108,7 @@
#include <geo/geo.h>
#include <systemlib/state_table.h>
#include <dataman/dataman.h>
#include <navigator/navigation.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -238,6 +239,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma
void set_control_mode();
bool stabilization_required();
void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
void print_reject_arm(const char *msg);
@@ -294,7 +297,7 @@ int commander_main(int argc, char *argv[])
SCHED_PRIORITY_MAX - 40,
3400,
commander_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
(char * const *)&argv[0]);
unsigned constexpr max_wait_us = 1000000;
unsigned constexpr max_wait_steps = 2000;
@@ -359,7 +362,7 @@ int commander_main(int argc, char *argv[])
if (calib_ret) {
warnx("calibration failed, exiting.");
return 0;
return 1;
} else {
return 0;
}
@@ -388,7 +391,7 @@ int commander_main(int argc, char *argv[])
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(false, mavlink_fd_local, "command line");
px4_close(mavlink_fd_local);
return 0;
return 0;
}
usage("unrecognized command");
@@ -874,6 +877,23 @@ int commander_thread_main(int argc, char *argv[])
bool arm_tune_played = false;
bool was_armed = false;
bool startup_in_hil = false;
#ifdef __PX4_NUTTX
/* NuttX indicates 3 arguments when only 2 are present */
argc -= 1;
#endif
if (argc > 2) {
if (!strcmp(argv[2],"-hil")) {
startup_in_hil = true;
} else {
PX4_ERR("Argument %s not supported.", argv[2]);
PX4_ERR("COMMANDER NOT STARTED");
thread_should_exit = true;
}
}
/* set parameters */
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
@@ -955,7 +975,12 @@ int commander_thread_main(int argc, char *argv[])
status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
if(startup_in_hil) {
status.hil_state = vehicle_status_s::HIL_STATE_ON;
} else {
status.hil_state = vehicle_status_s::HIL_STATE_OFF;
}
status.failsafe = false;
/* neither manual nor offboard control commands have been received */
@@ -2219,7 +2244,6 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
//fflush(stdout);
counter++;
int blink_state = blink_msg_state();
@@ -2537,8 +2561,8 @@ set_control_mode()
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
control_mode.flag_control_rates_enabled = stabilization_required();
control_mode.flag_control_attitude_enabled = stabilization_required();
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
@@ -2579,8 +2603,8 @@ set_control_mode()
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_termination_enabled = false;
break;
@@ -2689,12 +2713,12 @@ set_control_mode()
!offboard_control_mode.ignore_acceleration_force;
control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode;
!offboard_control_mode.ignore_position) && !status.in_transition_mode;
control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode;
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode;
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
@@ -2706,6 +2730,15 @@ set_control_mode()
}
}
bool
stabilization_required()
{
return (status.is_rotary_wing || // is a rotary wing, or
status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or
(vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND
!status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode
}
void
print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
{
src/modules/commander/state_machine_helper_posix.cpp
-634
View File
@@ -1,634 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"ARMING_STATE_INIT",
"ARMING_STATE_STANDBY",
"ARMING_STATE_ARMED",
"ARMING_STATE_ARMED_ERROR",
"ARMING_STATE_STANDBY_ERROR",
"ARMING_STATE_REBOOT",
"ARMING_STATE_IN_AIR_RESTORE",
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
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) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
int prearm_ret = OK;
/* only perform the check if we have to */
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
/*
* Perform an atomic state update
*/
//irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
/* 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 first!");
feedback_provided = true;
valid_transition = false;
}
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
}
}
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
}
// HIL can always go to standby
if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
warnx("HIL STATE ON -> STANDBY");
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
warnx("NOT ARMING: Sensors not operational.");
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}
// Finish up the state transition
if (valid_transition) {
armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
/* end of atomic state update */
//irqrestore(flags);
}
if (ret == TRANSITION_DENIED) {
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]);
/* 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;
}
bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
break;
case vehicle_status_s::MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
return ret;
}
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
break;
/* skip mavlink */
if (!strcmp(MAVLINK_LOG_DEVICE, devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown HIL state");
break;
}
}
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
// XXX also set lockdown here
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case vehicle_status_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case vehicle_status_s::MAIN_STATE_POSCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - depending on datalink, RC and if the mission is finished */
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
* check if both, RC and datalink are lost during the mission
* or RC is lost after the mission is finished: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
case vehicle_status_s::MAIN_STATE_AUTO_RTL:
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
break;
case vehicle_status_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
status->failsafe = true;
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
}
default:
break;
}
return status->nav_state != nav_state_old;
}
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
/* at this point this equals the preflight check, but might add additional
* quantities later.
*/
bool checkAirspeed = false;
/* 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) {
checkAirspeed = true;
}
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
}
src/modules/dataman/dataman.h
+1
View File
@@ -39,6 +39,7 @@
#ifndef _DATAMANAGER_H
#define _DATAMANAGER_H
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -3
View File
@@ -947,9 +947,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
if (!_local_pos.xy_global ||
!_local_pos.v_xy_valid ||
_gps.timestamp_position == 0 ||
(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_global_pos.eph = EPH_LARGE_VALUE;
_global_pos.epv = EPV_LARGE_VALUE;
} else {
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
@@ -1129,8 +1134,8 @@ int AttitudePositionEstimatorEKF::start()
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
7500,
SCHED_PRIORITY_MAX - 40,
4800,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
src/modules/ekf_att_pos_estimator/module.mk
+1 -1
View File
@@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
estimator_22states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400
src/modules/fw_att_control/fw_att_control_main.cpp
+40 -34
View File
@@ -42,6 +42,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -168,7 +171,6 @@ private:
float p_rmax_pos;
float p_rmax_neg;
float p_integrator_max;
float p_roll_feedforward;
float r_p;
float r_i;
float r_ff;
@@ -178,7 +180,6 @@ private:
float y_i;
float y_d;
float y_ff;
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated_min_speed;
int32_t y_coordinated_method;
@@ -211,7 +212,6 @@ private:
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
param_t p_roll_feedforward;
param_t r_p;
param_t r_i;
param_t r_ff;
@@ -221,7 +221,6 @@ private:
param_t y_i;
param_t y_d;
param_t y_ff;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t y_coordinated_min_speed;
param_t y_coordinated_method;
@@ -374,7 +373,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_i = param_find("FW_RR_I");
@@ -426,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -450,7 +448,6 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
@@ -493,7 +490,6 @@ FixedwingAttitudeControl::parameters_update()
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
@@ -671,8 +667,9 @@ FixedwingAttitudeControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -807,7 +804,7 @@ FixedwingAttitudeControl::task_main()
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
if (PX4_ISFINITE(_manual.flaps)) {
flaps_control = _manual.flaps;
}
@@ -817,7 +814,7 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
@@ -997,7 +994,7 @@ FixedwingAttitudeControl::task_main()
control_input.lock_integrator = lock_integrator;
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -1009,8 +1006,8 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
@@ -1020,8 +1017,8 @@ FixedwingAttitudeControl::task_main()
}
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@@ -1041,11 +1038,11 @@ FixedwingAttitudeControl::task_main()
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!isfinite(yaw_u)) {
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
@@ -1055,11 +1052,11 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
@@ -1139,7 +1136,6 @@ FixedwingAttitudeControl::task_main()
_control_task = -1;
_task_running = false;
_exit(0);
}
int
@@ -1152,7 +1148,7 @@ FixedwingAttitudeControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingAttitudeControl::task_main_trampoline,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@@ -1166,23 +1162,29 @@ FixedwingAttitudeControl::start()
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_att_control {start|stop|status}");
warnx("usage: fw_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (att_control::g_control != nullptr)
errx(1, "already running");
if (att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
att_control::g_control = new FixedwingAttitudeControl;
if (att_control::g_control == nullptr)
errx(1, "alloc failed");
if (att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
err(1, "start failed");
warn("start failed");
return 1;
}
/* check if the waiting is necessary at all */
@@ -1196,24 +1198,28 @@ int fw_att_control_main(int argc, char *argv[])
}
printf("\n");
}
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr)
errx(1, "not running");
if (att_control::g_control == nullptr){
warnx("not running");
return 1;
}
delete att_control::g_control;
att_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (att_control::g_control) {
errx(0, "running");
warnx("running");
return 0;
} else {
errx(1, "not running");
warnx("not running");
return 1;
}
}
src/modules/fw_att_control/fw_att_control_params.c
-11
View File
@@ -128,17 +128,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
/**
* Roll to Pitch feedforward gain.
*
* This compensates during turns and ensures the nose stays level.
*
* @min 0.0
* @max 2.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
/**
* Roll rate proportional Gain
*
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+49 -31
View File
@@ -52,6 +52,9 @@
*/
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
@@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
if (!isfinite(global_pos.terrain_alt)) {
if (!PX4_ISFINITE(global_pos.terrain_alt)) {
return land_setpoint_alt;
}
@@ -1024,7 +1027,11 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
_althold_epv = _global_pos.epv;
_was_in_deadband = true;
}
if (_vehicle_status.is_vtol) {
if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) {
_hold_alt = _global_pos.alt;
}
}
return climbout_mode;
}
@@ -1728,8 +1735,9 @@ FixedwingPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
@@ -1787,11 +1795,11 @@ FixedwingPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) {
if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) {
} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
@@ -1821,7 +1829,6 @@ FixedwingPositionControl::task_main()
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}
void FixedwingPositionControl::reset_takeoff_state()
@@ -1920,22 +1927,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
break;
}
t.altitudeSp = s.hgt_dem;
t.altitude_filtered = s.hgt;
t.airspeedSp = s.spd_dem;
t.airspeed_filtered = s.spd;
t.altitudeSp = s.altitude_sp;
t.altitude_filtered = s.altitude_filtered;
t.airspeedSp = s.airspeed_sp;
t.airspeed_filtered = s.airspeed_filtered;
t.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.flightPathAngleSp = s.altitude_rate_sp;
t.flightPathAngle = s.altitude_rate;
t.flightPathAngleFiltered = s.altitude_rate;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.airspeedDerivativeSp = s.airspeed_rate_sp;
t.airspeedDerivative = s.airspeed_rate;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
t.totalEnergyError = s.total_energy_error;
t.totalEnergyRateError = s.total_energy_rate_error;
t.energyDistributionError = s.energy_distribution_error;
t.energyDistributionRateError = s.energy_distribution_rate_error;
t.throttle_integ = s.throttle_integ;
t.pitch_integ = s.pitch_integ;
if (_tecs_status_pub != nullptr) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
@@ -1955,7 +1965,7 @@ FixedwingPositionControl::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
(main_t)&FixedwingPositionControl::task_main_trampoline,
(px4_main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
@@ -1969,16 +1979,20 @@ FixedwingPositionControl::start()
int fw_pos_control_l1_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
warnx("usage: fw_pos_control_l1 {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (l1_control::g_control != nullptr)
errx(1, "already running");
if (l1_control::g_control != nullptr) {
warnx("already running");
return 1;
}
if (OK != FixedwingPositionControl::start()) {
err(1, "start failed");
warn("start failed");
return 1;
}
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
@@ -1989,24 +2003,28 @@ int fw_pos_control_l1_main(int argc, char *argv[])
}
printf("\n");
exit(0);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (l1_control::g_control == nullptr)
errx(1, "not running");
if (l1_control::g_control == nullptr){
warnx("not running");
return 1;
}
delete l1_control::g_control;
l1_control::g_control = nullptr;
exit(0);
return 0;
}
if (!strcmp(argv[1], "status")) {
if (l1_control::g_control) {
errx(0, "running");
warnx("running");
return 0;
} else {
errx(1, "not running");
warnx("not running");
return 1;
}
}
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+11 -11
View File
@@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
_status.totalEnergyRate = totalEnergyRate;
_status.energyDistributionRateSp = energyDistributionRateSp;
_status.energyDistributionRate = energyDistributionRate;
_status.mode = mode;
// /* Write part of the status message */
// _status.flightPathAngleSp = flightPathAngleSp;
// _status.flightPathAngle = flightPathAngle;
// _status.flightPathAngleFiltered = flightPathAngleFiltered;
// _status.airspeedDerivativeSp = airspeedDerivativeSp;
// _status.airspeedDerivative = airspeedDerivative;
// _status.totalEnergyRateSp = totalEnergyRateSp;
// _status.totalEnergyRate = totalEnergyRate;
// _status.energyDistributionRateSp = energyDistributionRateSp;
// _status.energyDistributionRate = energyDistributionRate;
// _status.mode = mode;
/** update control blocks **/
/* update total energy rate control block */
src/modules/mavlink/mavlink_main.cpp
+27 -3
View File
@@ -1411,7 +1411,7 @@ Mavlink::task_main(int argc, char *argv[])
_datarate = 0;
_mode = MAVLINK_MODE_NORMAL;
#ifndef __PX4_POSIX
#ifdef __PX4_NUTTX
/* the NuttX optarg handler does not
* ignore argv[0] like the POSIX handler
* does, nor does it deal with non-flag
@@ -1484,6 +1484,8 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
} else if (strcmp(optarg, "config") == 0) {
_mode = MAVLINK_MODE_CONFIG;
}
break;
@@ -1600,8 +1602,6 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
_task_running = true;
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1693,6 +1693,27 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("VTOL_STATE", 0.5f);
break;
case MAVLINK_MODE_CONFIG:
// Enable a number of interesting streams we want via USB
configure_stream("PARAM_VALUE", 300.0f);
configure_stream("MISSION_ITEM", 50.0f);
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("VFR_HUD", 20.0f);
configure_stream("ATTITUDE", 100.0f);
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("RC_CHANNELS_RAW", 5.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("LOCAL_POSITION_NED", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
configure_stream("HIGHRES_IMU", 100.0f);
configure_stream("GPS_RAW_INT", 20.0f);
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("VTOL_STATE", 2.0f);
default:
break;
}
@@ -1877,6 +1898,9 @@ Mavlink::task_main(int argc, char *argv[])
}
perf_end(_loop_perf);
/* confirm task running only once fully initialized */
_task_running = true;
}
delete _subscribe_to_stream;
src/modules/mavlink/mavlink_main.h
+2 -1
View File
@@ -147,7 +147,8 @@ public:
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_ONBOARD,
MAVLINK_MODE_OSD
MAVLINK_MODE_OSD,
MAVLINK_MODE_CONFIG
};
void set_mode(enum MAVLINK_MODE);
src/modules/mavlink/mavlink_mission.cpp
+1
View File
@@ -49,6 +49,7 @@
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
src/modules/mavlink/mavlink_parameters.cpp
+2 -2
View File
@@ -159,9 +159,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
size_t i = map_rc.parameter_rc_channel_index;
_rc_param_map.param_index[i] = map_rc.param_index;
strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
_rc_param_map.scale[i] = map_rc.scale;
_rc_param_map.value0[i] = map_rc.param_value0;
_rc_param_map.value_min[i] = map_rc.param_value_min;
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -1050,7 +1050,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
/* channels */
@@ -1094,7 +1094,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
/* roll */
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -3
View File
@@ -993,9 +993,12 @@ MulticopterPositionControl::task_main()
reset_yaw_sp = true;
}
// XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition
if (_vehicle_status.in_transition_mode) {
reset_yaw_sp = true;
/* reset yaw and altitude setpoint for VTOL which are in fw mode */
if (_vehicle_status.is_vtol) {
if (!_vehicle_status.is_rotary_wing) {
reset_yaw_sp = true;
_reset_alt_sp = true;
}
}
//Update previous arming state
src/modules/navigator/datalinkloss.cpp
+1
View File
@@ -45,6 +45,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/enginefailure.cpp
+1
View File
@@ -44,6 +44,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/gpsfailure.cpp
+1
View File
@@ -45,6 +45,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/mission.cpp
+16 -12
View File
@@ -55,6 +55,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -624,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
{
/* select onboard/offboard mission */
int *mission_index_ptr;
struct mission_s *mission;
dm_item_t dm_item;
int mission_index_next;
struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
/* do not work on empty missions */
if (mission->count == 0) {
return false;
}
/* move to next item if there is one */
if (mission_index_next < ((int)mission->count - 1)) {
mission_index_next++;
}
if (onboard) {
/* onboard mission */
mission_index_next = _current_onboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
mission = &_onboard_mission;
dm_item = DM_KEY_WAYPOINTS_ONBOARD;
} else {
/* offboard mission */
mission_index_next = _current_offboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
mission = &_offboard_mission;
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
@@ -653,7 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
return false;
}
@@ -665,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* read mission item from datamanager */
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(),
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
"ERROR waypoint could not be read");
return false;
}
src/modules/navigator/mission_block.h
+2
View File
@@ -43,6 +43,8 @@
#include <drivers/drv_hrt.h>
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
src/modules/uORB/topics/mission.h → src/modules/navigator/navigation.h
+4 -13
View File
@@ -32,19 +32,18 @@
****************************************************************************/
/**
* @file mission.h
* @file navigation.h
* Definition of a mission consisting of mission items.
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_H_
#define TOPIC_MISSION_H_
#ifndef NAVIGATION_H_
#define NAVIGATION_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#define NUM_MISSIONS_SUPPORTED 256
@@ -103,15 +102,7 @@ struct mission_item_s {
int actuator_value; /**< new value for selected actuator in ms 900...2000 */
};
/**
* This topic used to notify navigator about mission changes, mission itself and new mission state
* must be stored in dataman before publication.
*/
struct mission_s {
int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
unsigned count; /**< count of the missions stored in the dataman */
int current_seq; /**< default -1, start at the one changed latest */
};
#include <uORB/topics/mission.h>
/**
* @}
src/modules/navigator/navigator.h
+1
View File
@@ -45,6 +45,7 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -524,7 +524,7 @@ Navigator::start()
/* start the task */
_navigator_task = px4_task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX -5,
SCHED_PRIORITY_DEFAULT - 5,
1500,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
src/modules/navigator/rcloss.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
src/modules/navigator/rtl.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
src/modules/navigator/rtl.h
+1 -2
View File
@@ -44,8 +44,7 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
src/modules/position_estimator_inav/position_estimator_inav_main.c
+3 -3
View File
@@ -116,10 +116,10 @@ static inline int max(int val1, int val2)
static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
PX4_INFO("%s\n", reason);
}
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
return;
}
@@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (isfinite(sensor.baro_alt_meter)) {
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
src/modules/sdlog2/sdlog2.c
+31 -13
View File
@@ -106,6 +106,7 @@
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/git_version.h>
#include <systemlib/printload.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
@@ -200,7 +201,8 @@ static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* GPS time, used for log files naming */
static uint64_t gps_time = 0;
static uint64_t gps_time_sec = 0;
static bool has_gps_3d_fix = false;
/* current state of logging */
static bool logging_enabled = false;
@@ -412,13 +414,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) {
/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
time_t utc_time_sec;
if (_gpstime_only) {
utc_time_sec = gps_time / 1e6;
if (_gpstime_only && has_gps_3d_fix) {
utc_time_sec = gps_time_sec;
} else {
utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
}
if (utc_time_sec > PX4_EPOCH_SECS) {
if (utc_time_sec > PX4_EPOCH_SECS) {
/* strip the time elapsed since boot */
if (boot_time) {
utc_time_sec -= hrt_absolute_time() / 1e6;
@@ -737,9 +739,16 @@ void sdlog2_start_log()
}
/* write all performance counters */
hrt_abstime curr_time = hrt_absolute_time();
struct print_load_s load;
int perf_fd = open_perf_file("preflight");
init_print_load_s(curr_time, &load);
print_load(curr_time, perf_fd, &load);
dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
perf_print_all(perf_fd);
dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n");
usleep(500 * 1000);
print_load(hrt_absolute_time(), perf_fd, &load);
close(perf_fd);
/* reset performance counters to get in-flight min and max values in post flight log */
@@ -775,8 +784,15 @@ void sdlog2_stop_log()
/* write all performance counters */
int perf_fd = open_perf_file("postflight");
hrt_abstime curr_time = hrt_absolute_time();
dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
perf_print_all(perf_fd);
struct print_load_s load;
dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n");
init_print_load_s(curr_time, &load);
print_load(curr_time, perf_fd, &load);
sleep(1);
print_load(hrt_absolute_time(), perf_fd, &load);
close(perf_fd);
/* free log writer performance counter */
@@ -989,7 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[])
sdlog2_usage(NULL);
}
gps_time = 0;
gps_time_sec = 0;
/* interpret logging params */
@@ -1289,7 +1305,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_utc_usec / 1e6;
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
}
}
@@ -1320,7 +1336,8 @@ int sdlog2_thread_main(int argc, char *argv[])
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time = buf_gps_pos.time_utc_usec / 1e6;
gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
has_gps_3d_fix = buf_gps_pos.fix_type == 3;
}
if (!logging_enabled) {
@@ -1900,15 +1917,16 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
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;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ;
log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ;
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
LOGBUFFER_WRITE_AND_COUNT(TECS);
}
@@ -1976,7 +1994,7 @@ int sdlog2_thread_main(int argc, char *argv[])
void sdlog2_status()
{
warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
warnx("time: gps: %u seconds", (unsigned)gps_time);
warnx("time: gps: %u seconds", (unsigned)gps_time_sec);
if (!logging_enabled) {
warnx("not logging");
} else {
src/modules/sdlog2/sdlog2_messages.h
+7 -7
View File
@@ -363,16 +363,16 @@ struct log_TECS_s {
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
float totalEnergyRateSp;
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
float totalEnergyError;
float totalEnergyRateError;
float energyDistributionError;
float energyDistributionRateError;
float pitch_integ;
float throttle_integ;
uint8_t mode;
};
@@ -545,7 +545,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, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
src/modules/sensors/sensor_params.c
+4 -4
View File
@@ -1887,7 +1887,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_ROLL, 0);
/**
* Pitch control channel mapping.
@@ -1900,7 +1900,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 0);
/**
* Failsafe channel mapping.
@@ -1927,7 +1927,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0);
/**
* Yaw control channel mapping.
@@ -1940,7 +1940,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
/**
* Mode switch channel mapping.
src/modules/sensors/sensors.cpp
+22 -23
View File
@@ -165,7 +165,7 @@ public:
int start();
private:
static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
@@ -239,7 +239,6 @@ private:
int32_t _mag2_prio; /**< mag2 sensor priority */
int32_t _baro_prio; /**< baro0 sensor priority */
int32_t _baro1_prio; /**< baro1 sensor priority */
int32_t _diff_pres_prio; /**< baro1 sensor priority */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -249,7 +248,7 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -294,7 +293,7 @@ private:
int rc_map_aux4;
int rc_map_aux5;
int rc_map_param[RC_PARAM_MAP_NCHAN];
int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
int32_t rc_fails_thr;
float rc_assist_th;
@@ -350,8 +349,8 @@ private:
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_map_param[RC_PARAM_MAP_NCHAN];
param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound
param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound
to a RC channel, equivalent float values in the
_parameters struct are not existing
because these parameters are never read. */
@@ -603,9 +602,9 @@ Sensors::Sensors() :
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
/* RC to parameter mapping for changing parameters with RC */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
char name[PARAM_ID_LEN];
snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
char name[rc_parameter_map_s::PARAM_ID_LEN];
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
_parameter_handles.rc_map_param[i] = param_find(name);
}
@@ -783,7 +782,7 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
}
@@ -831,7 +830,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
@@ -1544,6 +1543,12 @@ Sensors::parameter_update_poll(bool forced)
/* run through all mag sensors */
for (unsigned s = 0; s < 3; s++) {
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
* in the section below.
*/
_mag_rotation[s] = _board_rotation;
res = ERROR;
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
@@ -1554,12 +1559,6 @@ Sensors::parameter_update_poll(bool forced)
continue;
}
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
* in the section below.
*/
_mag_rotation[s] = _board_rotation;
bool config_ok = false;
/* run through all stored calibrations */
@@ -1696,7 +1695,7 @@ Sensors::rc_parameter_map_poll(bool forced)
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
@@ -1709,17 +1708,17 @@ Sensors::rc_parameter_map_poll(bool forced)
_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
_parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]);
}
}
warnx("rc to parameter map updated");
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
i,
_rc_parameter_map.param_id[i],
&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
(double)_rc_parameter_map.scale[i],
(double)_rc_parameter_map.value0[i],
(double)_rc_parameter_map.value_min[i],
@@ -1911,7 +1910,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
void
Sensors::set_params_from_rc()
{
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
@@ -2142,7 +2141,7 @@ Sensors::task_main()
warnx("Sensor initialization failed");
_sensors_task = -1;
if (_fd_adc >=0) {
close(_fd_adc);
px4_close(_fd_adc);
_fd_adc = -1;
}
return;
src/modules/systemlib/circuit_breaker_params.c
+1 -1
View File
@@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
/**
* Circuit breaker for IO safety
*
* Setting this parameter to 894281 will disable IO safety.
* Setting this parameter to 22027 will disable IO safety.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
src/modules/systemlib/module.mk
+4 -1
View File
@@ -55,11 +55,14 @@ SRCS = \
ifeq ($(PX4_TARGET_OS),nuttx)
SRCS += err.c \
printload.c \
up_cxxinitialize.c
else
SRCS += print_load_posix.c
endif
ifneq ($(PX4_TARGET_OS),qurt)
SRCS += hx_stream.c
SRCS += hx_stream.c
endif
MAXOPTIMIZATION = -Os
src/modules/systemlib/perf_counter.c
+1 -1
View File
@@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle)
void
perf_print_counter(perf_counter_t handle)
{
perf_print_counter_fd(0, handle);
perf_print_counter_fd(1, handle);
}
void
src/modules/systemlib/print_load_posix.c
+74
View File
@@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file print_load_posix.c
*
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <string.h>
#include <stdio.h>
#include <systemlib/cpuload.h>
#include <systemlib/printload.h>
#include <drivers/drv_hrt.h>
extern struct system_load_s system_load;
#define CL "\033[K" // clear line
void init_print_load_s(uint64_t t, struct print_load_s *s)
{
s->total_user_time = 0;
s->running_count = 0;
s->blocked_count = 0;
s->new_time = t;
s->interval_start_time = t;
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
s->last_times[i] = 0;
}
s->interval_time_ms_inv = 0.f;
}
void print_load(uint64_t t, int fd, struct print_load_s *print_state)
{
}
src/modules/systemlib/printload.c
+278
View File
@@ -0,0 +1,278 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file printload.c
*
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <string.h>
#include <stdio.h>
#include <systemlib/cpuload.h>
#include <systemlib/printload.h>
#include <drivers/drv_hrt.h>
extern struct system_load_s system_load;
#define CL "\033[K" // clear line
void init_print_load_s(uint64_t t, struct print_load_s *s)
{
s->total_user_time = 0;
s->running_count = 0;
s->blocked_count = 0;
s->new_time = t;
s->interval_start_time = t;
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
s->last_times[i] = 0;
}
s->interval_time_ms_inv = 0.f;
}
static const char *
tstate_name(const tstate_t s)
{
switch (s) {
case TSTATE_TASK_INVALID:
return "init";
case TSTATE_TASK_PENDING:
return "PEND";
case TSTATE_TASK_READYTORUN:
return "READY";
case TSTATE_TASK_RUNNING:
return "RUN";
case TSTATE_TASK_INACTIVE:
return "inact";
case TSTATE_WAIT_SEM:
return "w:sem";
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
return "w:sig";
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
return "w:mqe";
case TSTATE_WAIT_MQNOTFULL:
return "w:mqf";
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
return "w:pgf";
#endif
default:
return "ERROR";
}
}
void print_load(uint64_t t, int fd, struct print_load_s *print_state)
{
print_state->new_time = t;
int i;
uint64_t curr_time_us;
uint64_t idle_time_us;
char *clear_line = "";
/* print system information */
if (fd == 1) {
dprintf(fd, "\033[H"); /* move cursor home and clear screen */
clear_line = CL;
}
curr_time_us = t;
idle_time_us = system_load.tasks[0].total_runtime;
if (print_state->new_time > print_state->interval_start_time) {
print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000));
}
print_state->running_count = 0;
print_state->blocked_count = 0;
print_state->total_user_time = 0;
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
uint64_t interval_runtime;
if (system_load.tasks[i].valid) {
switch (system_load.tasks[i].tcb->task_state) {
case TSTATE_TASK_PENDING:
case TSTATE_TASK_READYTORUN:
case TSTATE_TASK_RUNNING:
print_state->running_count++;
break;
case TSTATE_TASK_INVALID:
case TSTATE_TASK_INACTIVE:
case TSTATE_WAIT_SEM:
#ifndef CONFIG_DISABLE_SIGNALS
case TSTATE_WAIT_SIG:
#endif
#ifndef CONFIG_DISABLE_MQUEUE
case TSTATE_WAIT_MQNOTEMPTY:
case TSTATE_WAIT_MQNOTFULL:
#endif
#ifdef CONFIG_PAGING
case TSTATE_WAIT_PAGEFILL:
#endif
print_state->blocked_count++;
break;
}
}
interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 &&
system_load.tasks[i].total_runtime > print_state->last_times[i])
? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000
: 0;
print_state->last_times[i] = system_load.tasks[i].total_runtime;
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv;
if (i > 0) {
print_state->total_user_time += interval_runtime;
}
} else {
print_state->curr_loads[i] = 0;
}
}
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
if (system_load.tasks[i].tcb->pid == 0) {
float idle;
float task_load;
float sched_load;
idle = print_state->curr_loads[0];
task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv;
/* this can happen if one tasks total runtime was not computed
correctly by the scheduler instrumentation TODO */
if (task_load > (1.f - idle)) {
task_load = (1.f - idle);
}
sched_load = 1.f - idle - task_load;
dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n",
clear_line,
system_load.total_count,
print_state->running_count,
print_state->blocked_count);
dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
clear_line,
(double)(task_load * 100.f),
(double)(sched_load * 100.f),
(double)(idle * 100.f));
dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n",
clear_line,
(double)curr_time_us / 1000000.d,
(double)idle_time_us / 1000000.d,
clear_line);
/* header for task list */
dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n",
clear_line,
"PID",
CONFIG_TASK_NAME_SIZE, "COMMAND",
"CPU(ms)",
"CPU(%)",
"USED/STACK",
"PRIO(BASE)",
#if CONFIG_RR_INTERVAL > 0
"TSLICE"
#else
"STATE"
#endif
);
}
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
unsigned stack_free = 0;
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
while (stack_free < stack_size) {
if (*stack_sweeper++ != 0xff) {
break;
}
stack_free++;
}
dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
clear_line,
system_load.tasks[i].tcb->pid,
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
(system_load.tasks[i].total_runtime / 1000),
(int)(print_state->curr_loads[i] * 100.0f),
(int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000),
stack_size - stack_free,
stack_size,
system_load.tasks[i].tcb->sched_priority,
system_load.tasks[i].tcb->base_priority);
#if CONFIG_RR_INTERVAL > 0
/* print scheduling info with RR time slice */
dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice);
#else
// print task state instead
dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
#endif
}
}
print_state->interval_start_time = print_state->new_time;
}
src/modules/uORB/topics/rc_parameter_map.h → src/modules/systemlib/printload.h
+25 -31
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -20,7 +20,7 @@
* "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 ,
* 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
@@ -32,45 +32,39 @@
****************************************************************************/
/**
* @file rc_parameter_map.h
* Maps RC channels to parameters
* @file printload.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* Print the current system load.
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#ifndef TOPIC_RC_PARAMETER_MAP_H
#define TOPIC_RC_PARAMETER_MAP_H
#pragma once
__BEGIN_DECLS
#include <px4_config.h>
#include <stdint.h>
#include "../uORB.h"
#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
#ifndef CONFIG_MAX_TASKS
#define CONFIG_MAX_TASKS 64
#endif
/**
* @addtogroup topics
* @{
*/
struct print_load_s {
uint64_t total_user_time;
struct rc_parameter_map_s {
uint64_t timestamp; /**< time at which the map was updated */
int running_count;
int blocked_count;
bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */
int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this
this field is ignored if set to -1, in this case param_id will
be used*/
char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */
float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */
float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */
float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */
float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */
uint64_t new_time;
uint64_t interval_start_time;
uint64_t last_times[CONFIG_MAX_TASKS];
float curr_loads[CONFIG_MAX_TASKS];
float interval_time_ms_inv;
};
/**
* @}
*/
__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s);
ORB_DECLARE(rc_parameter_map);
__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state);
#endif
__END_DECLS
src/modules/uORB/module.mk
+3 -6
View File
@@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm)
SRCS += uORBTest_UnitTest.cpp
endif
ifneq ($(PX4_TARGET_OS),qurt)
SRCS += Publication.cpp \
Subscription.cpp
endif
SRCS += objects_common.cpp \
uORBUtils.cpp \
uORB.cpp \
uORBMain.cpp
uORBMain.cpp \
Publication.cpp \
Subscription.cpp
MAXOPTIMIZATION = -Os
src/modules/uORB/objects_common.cpp
+16 -12
View File
@@ -45,23 +45,23 @@
#include <drivers/drv_orb_dev.h>
#include <drivers/drv_mag.h>
ORB_DEFINE(sensor_mag, struct mag_report);
#include "topics/sensor_mag.h"
ORB_DEFINE(sensor_mag, struct sensor_mag_s);
#include <drivers/drv_accel.h>
ORB_DEFINE(sensor_accel, struct accel_report);
#include "topics/sensor_accel.h"
ORB_DEFINE(sensor_accel, struct sensor_accel_s);
#include <drivers/drv_gyro.h>
ORB_DEFINE(sensor_gyro, struct gyro_report);
#include "topics/sensor_gyro.h"
ORB_DEFINE(sensor_gyro, struct sensor_gyro_s);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
#include "topics/sensor_baro.h"
ORB_DEFINE(sensor_baro, struct sensor_baro_s);
#include <drivers/drv_pwm_output.h>
ORB_DEFINE(output_pwm, struct pwm_output_values);
#include "topics/output_pwm.h"
ORB_DEFINE(output_pwm, struct output_pwm_s);
#include <drivers/drv_rc_input.h>
ORB_DEFINE(input_rc, struct rc_input_values);
#include "topics/input_rc.h"
ORB_DEFINE(input_rc, struct input_rc_s);
#include "topics/pwm_input.h"
ORB_DEFINE(pwm_input, struct pwm_input_s);
@@ -140,6 +140,10 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
#include "topics/mission.h"
ORB_DEFINE(mission, struct mission_s);
// XXX onboard and offboard mission are still declared here until this is
// generator supported
#include <navigator/navigation.h>
ORB_DEFINE(offboard_mission, struct mission_s);
ORB_DEFINE(onboard_mission, struct mission_s);
src/modules/uORB/uORB.h
+1 -1
View File
@@ -338,7 +338,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
* priority, independent of the startup order of the associated publishers.
* @return OK on success, ERROR otherwise with errno set accordingly.
*/
extern int orb_priority(int handle, int *priority) __EXPORT;
extern int orb_priority(int handle, int32_t *priority) __EXPORT;
/**
* Set the minimum interval between which updates are seen for a subscription.
src/modules/uavcan/actuators/esc.cpp
+21 -13
View File
@@ -68,8 +68,8 @@ int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("ESC status sub failed %i", res);
return res;
}
@@ -83,9 +83,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* 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;
/*
@@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
if (scaled < 0.0F) {
scaled = 0.0F;
}
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
@@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
@@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
@@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
@@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub != nullptr) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
src/modules/uavcan/actuators/esc.hpp
+4 -4
View File
@@ -54,7 +54,7 @@
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
int init();
@@ -79,12 +79,12 @@ private:
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};
src/modules/uavcan/module.mk
+6 -1
View File
@@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400
# Main
SRCS += uavcan_main.cpp \
uavcan_servers.cpp \
uavcan_clock.cpp \
uavcan_params.c
@@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(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
override EXTRADEFINES := $(EXTRADEFINES) \
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
-DUAVCAN_NO_ASSERTIONS \
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
#
# libuavcan drivers for STM32
src/modules/uavcan/sensors/baro.cpp
+3 -5
View File
@@ -45,9 +45,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
_reports(nullptr)
{
last_temperature = 0.0f;
}
{ }
int UavcanBarometerBridge::init()
{
@@ -153,7 +151,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
void UavcanBarometerBridge::air_temperature_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{
last_temperature = msg.static_temperature;
last_temperature_kelvin = msg.static_temperature;
}
void UavcanBarometerBridge::air_pressure_sub_cb(const
@@ -162,7 +160,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = last_temperature;
report.temperature = last_temperature_kelvin - 273.15F;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
src/modules/uavcan/sensors/baro.hpp
+1 -1
View File
@@ -78,6 +78,6 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports;
float last_temperature;
float last_temperature_kelvin = 0.0f;
};
src/modules/uavcan/sensors/gnss.cpp
+17 -11
View File
@@ -46,20 +46,21 @@
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_sub_fix(node),
_report_pub(nullptr)
_node(node),
_sub_fix(node),
_report_pub(nullptr)
{
}
int UavcanGnssBridge::init()
{
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("GNSS fix sub failed %i", res);
return res;
}
return res;
}
@@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
void UavcanGnssBridge::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
@@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
@@ -114,14 +118,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
// 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);
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.
@@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
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));
(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;
@@ -154,7 +159,8 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
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.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;
src/modules/uavcan/sensors/mag.cpp
+39 -26
View File
@@ -41,11 +41,11 @@
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
@@ -55,15 +55,18 @@ _sub_mag(node)
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
DEVICE_LOG("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
@@ -74,6 +77,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
/* buffer must be large enough */
unsigned count = buflen / sizeof(struct mag_report);
if (count < 1) {
return -ENOSPC;
}
@@ -85,6 +89,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
last_read = _report.timestamp;
unlock();
return sizeof(struct mag_report);
} else {
/* no new data available, warn caller */
return -EAGAIN;
@@ -95,25 +100,31 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
{
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
return OK; // Pretend that this stuff is supported to keep APM happy
}
return OK; // Pretend that this stuff is supported to keep APM happy
}
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
std::memcpy(&_scale, reinterpret_cast<const void *>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
@@ -121,23 +132,25 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
_report.timestamp = msg.getMonotonicTimestamp().toUSec();
_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
unlock();
publish(msg.getSrcNodeID().get(), &_report);
src/modules/uavcan/sensors/mag.hpp
+4 -4
View File
@@ -40,7 +40,7 @@
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
@@ -57,14 +57,14 @@ private:
ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
mag_scale _scale = {};
mag_report _report = {};
};
src/modules/uavcan/sensors/sensor_bridge.cpp
+7 -1
View File
@@ -45,7 +45,7 @@
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
@@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _channels;
}
@@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
DEVICE_LOG("out of class instances");
@@ -128,6 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -136,11 +139,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
@@ -152,6 +157,7 @@ void UavcanCDevSensorBridgeBase::print_status() const
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}
src/modules/uavcan/sensors/sensor_bridge.hpp
+10 -11
View File
@@ -45,7 +45,7 @@
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
@@ -77,7 +77,7 @@ public:
* Sensor bridge factory.
* Creates all known sensor bridges and puts them in the linked list.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
};
/**
@@ -86,8 +86,7 @@ public:
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
struct Channel {
int node_id = -1;
orb_advert_t orb_advert = nullptr;
int class_instance = -1;
@@ -104,13 +103,13 @@ protected:
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
{
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;
src/modules/uavcan/uavcan_main.cpp
+211 -113
View File
@@ -53,15 +53,12 @@
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
#include <uavcan/util/templates.hpp>
#if defined(USE_FW_NODE_SERVER)
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
#endif
/**
* @file uavcan_main.cpp
@@ -76,33 +73,28 @@
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
#if defined(USE_FW_NODE_SERVER)
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
uavcan_posix::FirmwareVersionChecker fw_version_checker;
#endif
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node_mutex(),
#if !defined(USE_FW_NODE_SERVER)
_esc_controller(_node)
#else
_esc_controller(_node),
_fileserver_backend(_node),
_node_info_retriever(_node),
_fw_upgrade_trigger(_node, fw_version_checker),
_fw_server(_node, _fileserver_backend)
#endif
{
_task_should_exit = false;
_fw_server_action = None;
_fw_server_status = -1;
_tx_injector = nullptr;
_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);
const int res = pthread_mutex_init(&_node_mutex, nullptr);
int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
res = sem_init(&_server_command_sem, 0 , 0);
if (res < 0) {
std::abort();
@@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
@@ -161,13 +157,127 @@ UavcanNode::~UavcanNode()
perf_free(_perfcnt_node_spin_elapsed);
perf_free(_perfcnt_esc_mixer_output_elapsed);
perf_free(_perfcnt_esc_mixer_total_elapsed);
#if defined(USE_FW_NODE_SERVER)
delete(_server_instance);
#endif
pthread_mutex_destroy(&_node_mutex);
sem_destroy(&_server_command_sem);
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
{
int rv = -1;
if (UavcanNode::instance()) {
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
rv = 0;
}
return rv;
}
int UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(2, _node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action == None) {
_fw_server_action = action;
sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
@@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
return -1;
}
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
@@ -248,7 +363,7 @@ void UavcanNode::fill_node_info()
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
@@ -256,21 +371,7 @@ void UavcanNode::fill_node_info()
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
getHardwareVersion(hwver);
_node.setHardwareVersion(hwver);
}
@@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
br = br->getSibling();
}
#if defined(USE_FW_NODE_SERVER)
/* Initialize the fw version checker.
* giving it it's path
*/
ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
return ret;
}
/* Create dynamic node id server for the Firmware updates directory */
_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
if (_server_instance == 0) {
return -ENOMEM;
}
/* Initialize the dynamic node id server */
ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
if (ret < 0) {
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
if (ret < 0) {
return ret;
}
#endif
/* Start the Node */
return _node.start();
@@ -398,6 +431,11 @@ void UavcanNode::node_spin_once()
warnx("node spin error %i", spin_res);
}
if (_tx_injector != nullptr) {
_tx_injector->injectTxFramesInto(_node);
}
perf_end(_perfcnt_node_spin_elapsed);
}
@@ -446,7 +484,7 @@ int UavcanNode::run()
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
@@ -466,6 +504,24 @@ int UavcanNode::run()
while (!_task_should_exit) {
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
break;
case Stop:
_fw_server_status = stop_fw_server();
break;
case CheckFW:
_fw_server_status = request_fw_check();
break;
case None:
default:
break;
}
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
@@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
int
UavcanNode::teardown()
{
sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
@@ -812,7 +870,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start|status|stop|arm|disarm}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[])
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
warnx("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
warnx("already started");
::exit(0);
@@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[])
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
::exit(0);
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
@@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[])
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
if (rv < 0) {
warnx("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
}
print_usage();
src/modules/uavcan/uavcan_main.hpp
+14 -26
View File
@@ -48,13 +48,7 @@
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
#if defined(USE_FW_NODE_SERVER)
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
#endif
# include "uavcan_servers.hpp"
/**
* @file uavcan_main.hpp
@@ -65,10 +59,6 @@
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
@@ -84,8 +74,7 @@ class UavcanNode : public device::CDev
static constexpr unsigned PollTimeoutMs = 10;
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -99,11 +88,12 @@ class UavcanNode : public device::CDev
*/
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 3400;
static constexpr unsigned StackSize = 1600;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
@@ -126,6 +116,9 @@ public:
void print_info();
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
private:
void fill_node_info();
@@ -133,10 +126,14 @@ private:
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
volatile eServerAction _fw_server_action;
int _fw_server_status;
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
@@ -151,22 +148,13 @@ private:
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
sem_t _server_command_sem;
UavcanEscController _esc_controller;
#if defined(USE_FW_NODE_SERVER)
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
#endif
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
ITxQueueInjector *_tx_injector;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
src/modules/uavcan/uavcan_params.c
+5 -2
View File
@@ -41,10 +41,13 @@
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
* Allowed values:
* 0 - UAVCAN disabled.
* 1 - Enabled support for UAVCAN actuators and sensors.
* 2 - Enabled support for dynamic node ID allocation and firmware update.
*
* @min 0
* @max 1
* @max 2
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
src/modules/uavcan/uavcan_servers.cpp
+293
View File
@@ -0,0 +1,293 @@
/****************************************************************************
*
* 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 <memory>
#include <pthread.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/git_version.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include "uavcan_main.hpp"
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.hpp"
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
/**
* @file uavcan_servers.cpp
*
* Implements basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* David Sidrane <david_s5@nscdg.com>
*/
/*
* UavcanNode
*/
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_main_node(main_node),
_tracer(),
_storage_backend(),
_fw_version_checker(),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_mutex_inited(false),
_check_fw(false)
{
}
UavcanServers::~UavcanServers()
{
if (_mutex_inited) {
(void)Lock::deinit(_subnode_mutex);
}
_main_node.getDispatcher().removeRxFrameListener();
}
int UavcanServers::stop(void)
{
UavcanServers *server = instance();
if (server == nullptr) {
warnx("Already stopped");
return -1;
}
_instance = nullptr;
if (server->_subnode_thread != -1) {
pthread_cancel(server->_subnode_thread);
pthread_join(server->_subnode_thread, NULL);
}
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
return 0;
}
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init(num_ifaces);
if (rv < 0) {
warnx("Node init failed: %d", rv);
delete _instance;
_instance = nullptr;
return rv;
}
/*
* Start the thread. Normally it should never exit.
*/
pthread_attr_t tattr;
struct sched_param param;
pthread_attr_init(&tattr);
tattr.stacksize = StackSize;
param.sched_priority = Priority;
pthread_attr_setschedparam(&tattr, &param);
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
if (rv < 0) {
warnx("pthread_create() failed: %d", errno);
rv = -errno;
delete _instance;
_instance = nullptr;
}
return rv;
}
int UavcanServers::init(unsigned num_ifaces)
{
errno = 0;
/*
* Initialize the mutex.
* giving it its path
*/
int ret = Lock::init(_subnode_mutex);
if (ret < 0) {
warnx("Lock init: %d", errno);
return ret;
}
_mutex_inited = true;
_subnode.setNodeID(_main_node.getNodeID());
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
/*
* Initialize the fw version checker.
* giving it its path
*/
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
warnx("BasicFileServer init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = _tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
warnx("FileEventTracer init: %d, errno: %d", ret, errno);
return ret;
}
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
UavcanNode::getHardwareVersion(hwver);
/* Initialize the dynamic node id server */
ret = _server_instance.init(hwver.unique_id);
if (ret < 0) {
warnx("CentralizedServer init: %d", ret);
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
warnx("NodeInfoRetriever init: %d", ret);
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
if (ret < 0) {
warnx("FirmwareUpdateTrigger init: %d", ret);
return ret;
}
/* Start the Node */
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
Lock lock(_subnode_mutex);
while (1) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
warnx("exiting.");
return (pthread_addr_t) 0;
}
src/modules/uavcan/uavcan_servers.hpp
+145
View File
@@ -0,0 +1,145 @@
/****************************************************************************
*
* 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 <systemlib/perf_counter.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
# include "uavcan_virtual_can_driver.hpp"
/**
* @file uavcan_servers.hpp
*
* Defines basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author David Sidrane <david_s5@nscdg.com>
*/
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
/**
* A UAVCAN Server Sub node.
*/
class UavcanServers
{
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramsPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
* MemPoolBlockSize set at 48 this is 6048 Bytes.
*
* The servers can be forced to use the primary interface only, this can be achieved simply by passing
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
public:
UavcanServers(uavcan::INode &main_node);
virtual ~UavcanServers();
static int start(unsigned num_ifaces, uavcan::INode &main_node);
static int stop(void);
SubNode &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
/*
* Set the main node's pointer to to the injector
* This is a work around as main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
int init(unsigned num_ifaces);
void deinit(void);
pthread_addr_t run(pthread_addr_t);
static UavcanServers *_instance; ///< singleton pointer
typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
vCanDriver _vdriver;
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
uavcan::INode &_main_node; ///< library instance
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
uavcan_posix::FirmwareVersionChecker _fw_version_checker;
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
bool _mutex_inited;
volatile bool _check_fw;
};
src/modules/uavcan/uavcan_virtual_can_driver.hpp
+506
View File
@@ -0,0 +1,506 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Pavel Kirienko <pavel.kirienko@zubax.com>
* David Sidrane <david_s5@nscdg.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.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <cstdlib>
#include <cstdint>
#include <cstring>
#include <cstdio>
#include <fcntl.h>
#include <pthread.h>
#include <semaphore.h>
#include <debug.h>
#include <uavcan_stm32/clock.hpp>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
/*
* General purpose wrapper around os's mutual exclusion
* mechanism.
*
* It supports the
*
* Note the naming of thier_mutex_ implies that the underlying
* mutex is owned by class using the Lock.
* and this wrapper provides service to initialize and de initialize
* the mutex.
*/
class Lock
{
pthread_mutex_t &thier_mutex_;
public:
Lock(pthread_mutex_t &m) :
thier_mutex_(m)
{
(void)pthread_mutex_lock(&m);
}
~Lock()
{
(void)pthread_mutex_unlock(&thier_mutex_);
}
static int init(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_init(&thier_mutex_, NULL);
}
static int deinit(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_destroy(&thier_mutex_);
}
};
/**
* Generic queue based on the linked list class defined in libuavcan.
* This class does not use heap memory.
*/
template <typename T>
class Queue
{
struct Item : public uavcan::LinkedListNode<Item> {
T payload;
template <typename... Args>
Item(Args... args) : payload(args...) { }
};
uavcan::LimitedPoolAllocator allocator_;
uavcan::LinkedListRoot<Item> list_;
public:
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
allocator_(arg_allocator, block_allocation_quota)
{
uavcan::IsDynamicallyAllocatable<Item>::check();
}
bool isEmpty() const { return list_.isEmpty(); }
/**
* Creates one item in-place at the end of the list.
* Returns true if the item was appended successfully, false if there's not enough memory.
* Complexity is O(N) where N is queue length.
*/
template <typename... Args>
bool tryEmplace(Args... args)
{
// Allocating memory
void *const ptr = allocator_.allocate(sizeof(Item));
if (ptr == nullptr) {
return false;
}
// Constructing the new item
Item *const item = new(ptr) Item(args...);
assert(item != nullptr);
// Inserting the new item at the end of the list
Item *p = list_.get();
if (p == nullptr) {
list_.insert(item);
} else {
while (p->getNextListNode() != nullptr) {
p = p->getNextListNode();
}
assert(p->getNextListNode() == nullptr);
p->setNextListNode(item);
assert(p->getNextListNode()->getNextListNode() == nullptr);
}
return true;
}
/**
* Accesses the first element.
* Nullptr will be returned if the queue is empty.
* Complexity is O(1).
*/
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
/**
* Removes the first element.
* If the queue is empty, nothing will be done and assertion failure will be triggered.
* Complexity is O(1).
*/
void pop()
{
Item *const item = list_.get();
assert(item != nullptr);
if (item != nullptr) {
list_.remove(item);
item->~Item();
allocator_.deallocate(item);
}
}
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
*/
class VirtualCanIface : public uavcan::ICanIface,
uavcan::Noncopyable
{
/**
* This class re-defines uavcan::RxCanFrame with flags.
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
* rendering this class unusable with Queue<>.
*/
struct RxItem: public uavcan::CanFrame {
const uavcan::MonotonicTime ts_mono;
const uavcan::UtcTime ts_utc;
const uavcan::CanIOFlags flags;
const uint8_t iface_index;
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
uavcan::CanFrame(arg_frame),
ts_mono(arg_frame.ts_mono),
ts_utc(arg_frame.ts_utc),
flags(arg_flags),
iface_index(arg_frame.iface_index)
{
// Making sure it will fit into a pool block with a pointer prefix
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
}
};
pthread_mutex_t &common_driver_mutex_;
uavcan::CanTxQueue prioritized_tx_queue_;
Queue<RxItem> rx_queue_;
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
{
Lock lock(common_driver_mutex_);
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
return 1;
}
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
{
Lock lock(common_driver_mutex_);
if (rx_queue_.isEmpty()) {
return 0;
}
const auto item = *rx_queue_.peek();
rx_queue_.pop();
out_frame = item;
out_ts_monotonic = item.ts_mono;
out_ts_utc = item.ts_utc;
out_flags = item.flags;
return 1;
}
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
uint16_t getNumFilters() const override { return 0; }
uint64_t getErrorCount() const override { return 0; }
public:
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
common_driver_mutex_(arg_mutex),
prioritized_tx_queue_(allocator, clock, quota_per_queue),
rx_queue_(allocator, quota_per_queue)
{
}
~VirtualCanIface()
{
}
/**
* Note that RX queue overwrites oldest items when overflowed.
* Call this from the main thread only.
* No additional locking is required.
*/
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
{
Lock lock(common_driver_mutex_);
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
rx_queue_.pop();
(void)rx_queue_.tryEmplace(frame, flags);
}
}
/**
* Call this from the main thread only.
* No additional locking is required.
*/
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
{
Lock lock(common_driver_mutex_);
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
while (auto e = prioritized_tx_queue_.peek()) {
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
unsigned(iface_mask), e->toString().c_str());
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
uavcan::CanTxQueue::Qos(e->qos), e->flags);
prioritized_tx_queue_.remove(e);
if (res <= 0) {
break;
}
}
}
/**
* Call this from the sub-node thread only.
* No additional locking is required.
*/
bool hasDataInRxQueue()
{
Lock lock(common_driver_mutex_);
return !rx_queue_.isEmpty();
}
};
/**
* This interface defines one method that will be called by the main node thread periodically in order to
* transfer contents of TX queue of the sub-node into the TX queue of the main node.
*/
class ITxQueueInjector
{
public:
virtual ~ITxQueueInjector() { }
/**
* Flush contents of TX queues into the main node.
* @param main_node Reference to the main node.
*/
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
* @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
* memory pool that will be shared across all interfaces for RX/TX queues.
* Typically this value should be no less than 4K per interface.
*/
template <unsigned SharedMemoryPoolSize>
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
uavcan::Noncopyable
{
class Event
{
FAR sem_t sem;
public:
int init()
{
return sem_init(&sem, 0, 0);
}
int deinit()
{
return sem_destroy(&sem);
}
Event()
{
}
~Event()
{
}
/**
*/
void waitFor(uavcan::MonotonicDuration duration)
{
static const int NsPerSec = 1000000000;
if (duration.isPositive()) {
auto abstime = ::timespec();
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
abstime.tv_nsec += duration.toUSec() * 1000;
if (abstime.tv_nsec >= NsPerSec) {
abstime.tv_sec++;
abstime.tv_nsec -= NsPerSec;
}
(void)sem_timedwait(&sem, &abstime);
}
}
}
void signal()
{
int count;
int rv = sem_getvalue(&sem, &count);
if (rv > 0 && count <= 0) {
sem_post(&sem);
}
}
};
Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
uavcan::ICanIface *getIface(uint8_t iface_index) override
{
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
}
uint8_t getNumIfaces() const override { return num_ifaces_; }
/**
* This and other methods of ICanDriver will be invoked by the sub-node thread.
*/
int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
const bool need_read = inout_masks.read & (1U << i);
if (need_read && ifaces_[i]->hasDataInRxQueue()) {
need_block = false;
}
}
if (need_block) {
event_.waitFor(blocking_deadline - clock_.getMonotonic());
}
inout_masks = uavcan::CanSelectMasks();
for (unsigned i = 0; i < num_ifaces_; i++) {
const std::uint8_t iface_mask = 1U << i;
inout_masks.write |= iface_mask; // Always ready to write
if (ifaces_[i]->hasDataInRxQueue()) {
inout_masks.read |= iface_mask;
}
}
return num_ifaces_; // We're always ready to write, hence > 0.
}
/**
* This handler will be invoked by the main node thread.
*/
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
{
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
if (frame.iface_index < num_ifaces_) {
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
event_.signal();
}
}
/**
* This method will be invoked by the main node thread.
*/
void injectTxFramesInto(uavcan::INode &main_node) override
{
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i]->flushTxQueueTo(main_node, i);
}
event_.signal();
}
public:
VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
Lock::init(driver_mutex_);
event_.init();
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
}
}
~VirtualCanDriver()
{
Lock::deinit(driver_mutex_);
event_.deinit();
}
};
src/modules/vtol_att_control/tiltrotor.cpp
+6 -2
View File
@@ -342,10 +342,14 @@ void Tiltrotor::fill_actuator_outputs()
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
}
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw
_actuators_out_1->control[4] = _tilt_control;