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

Merge commit '2780dc39ce5d47f2d9dfa921062100a1dc86c2be' into mpc_track

Browse Source
This commit is contained in:
Anton Babushkin
2014-09-01 15:19:42 +02:00
parent f31c3243b0 2780dc39ce
commit 3c7b9ef94d
76 changed files with 2186 additions and 492 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/airspeed_calibration.cpp
+10 -8
View File
@@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
@@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
src/modules/commander/commander.cpp
+6 -1
View File
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
#define DL_TIMEOUT (10 * 1000 * 1000)
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
@@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case VEHICLE_CMD_PREFLIGHT_STORAGE:
case VEHICLE_CMD_CUSTOM_0:
case VEHICLE_CMD_CUSTOM_1:
case VEHICLE_CMD_CUSTOM_2:
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
/* ignore commands that handled in low prio loop */
break;
src/modules/commander/state_machine_helper.cpp
+13 -6
View File
@@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// 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) &&
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
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;
}
}
}
src/modules/fw_att_control/fw_att_control_main.cpp
+11 -8
View File
@@ -145,6 +145,7 @@ private:
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
_setpoint_valid(false)
_setpoint_valid(false),
_debug(false)
{
/* safely initialize structs */
_att = {};
@@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed.true_airspeed_m_s;
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
@@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+10 -1
View File
@@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
}
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
@@ -823,7 +829,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
return rel_alt_estimated;
}
@@ -1380,6 +1386,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+9 -4
View File
@@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
/**
* Landing flare altitude (relative)
* Landing flare altitude (relative to landing altitude)
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
/**
* Landing throttle limit altitude (relative)
* Landing throttle limit altitude (relative landing altitude)
*
* Default of -1.0f lets the system default to applying throttle
* limiting at 2/3 of the flare altitude.
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
/**
* Landing heading hold horizontal distance
src/modules/mavlink/mavlink_ftp.cpp
+8 -2
View File
@@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req)
uint32_t messageCRC;
// basic sanity checks; must validate length before use
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
if (hdr->size > kMaxDataLength) {
errorCode = kErrNoRequest;
goto out;
}
@@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req)
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
hdr->padding[0] = 0;
hdr->padding[1] = 0;
hdr->padding[2] = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
@@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
hdr->magic = kProtocolMagic;
hdr->seqNumber = req->header()->seqNumber + 1;
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
hdr->padding[0] = 0;
hdr->padding[1] = 0;
hdr->padding[2] = 0;
hdr->crc32 = crc32(req->rawData(), req->dataSize());
// then pack and send the reply back to the request source
src/modules/mavlink/mavlink_ftp.h
+28 -22
View File
@@ -38,9 +38,6 @@
*
* MAVLink remote file server.
*
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
* a session ID and sequence number.
*
* A limited number of requests (currently 2) may be outstanding at a time.
* Additional messages will be discarded.
*
@@ -74,16 +71,19 @@ private:
static MavlinkFTP *_server;
/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
/// structure ourselves to 32 bit alignment which should get us what we want.
struct RequestHeader
{
uint8_t magic;
uint8_t session;
uint8_t opcode;
uint8_t size;
uint32_t crc32;
uint32_t offset;
{
uint16_t seqNumber; ///< sequence number for message
uint8_t session; ///< Session id for read and write commands
uint8_t opcode; ///< Command opcode
uint8_t size; ///< Size of data
uint8_t padding[3];
uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
uint32_t offset; ///< Offsets for List and Read commands
uint8_t data[];
};
};
enum Opcode : uint8_t
{
@@ -131,10 +131,11 @@ private:
};
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
_systemId = fromMessage->sysid;
_mavlink = mavlink;
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
return true;
mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
return _message.target_system == _mavlink->get_system_id();
}
return false;
}
@@ -145,8 +146,14 @@ private:
// flat memory architecture, as we're operating between threads here.
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence()+1, rawData());
unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id
_mavlink->get_component_id(), // Sender component id
_mavlink->get_channel(), // Channel to send on
&msg, // Message to pack payload into
0, // Target network
_systemId, // Target system id
0, // Target component id
rawData()); // Payload to pack into message
_mavlink->lockMessageBufferMutex();
bool success = _mavlink->message_buffer_write(&msg, len);
@@ -167,26 +174,25 @@ private:
#endif
}
uint8_t *rawData() { return &_message.data[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
uint8_t *rawData() { return &_message.payload[0]; }
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
uint8_t *requestData() { return &(header()->data[0]); }
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
uint16_t sequence() const { return _message.seqnr; }
mavlink_channel_t channel() { return _mavlink->get_channel(); }
char *dataAsCString();
private:
Mavlink *_mavlink;
mavlink_encapsulated_data_t _message;
mavlink_file_transfer_protocol_t _message;
uint8_t _systemId;
};
static const uint8_t kProtocolMagic = 'f';
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.
src/modules/mavlink/mavlink_main.cpp
+52 -44
View File
@@ -134,44 +134,44 @@ Mavlink::Mavlink() :
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
_bytes_timestamp(0),
_rate_tx(0.0f),
_rate_txerr(0.0f),
_rate_rx(0.0f),
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
_bytes_timestamp(0),
_rate_tx(0.0f),
_rate_txerr(0.0f),
_rate_rx(0.0f),
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range");
break;
}
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
// left in here for compatibility
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
}
break;
@@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
case MAVLINK_MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
default:
@@ -1391,13 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 20.0f);
break;
case MAVLINK_MODE_CAMERA:
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);
configure_stream("CAMERA_CAPTURE", 1.0f);
configure_stream("ATTITUDE", 50.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
break;
default:
@@ -1653,6 +1659,8 @@ Mavlink::display_status()
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
printf("\tmavlink chan: #%u\n", _channel);
if (_rstatus.timestamp > 0) {
printf("\ttype:\t\t");
src/modules/mavlink/mavlink_main.h
+1 -1
View File
@@ -127,7 +127,7 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_CAMERA
MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);
src/modules/mavlink/mavlink_messages.cpp
+22 -15
View File
@@ -774,6 +774,9 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_combined_sub;
uint64_t _sensor_combined_time;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -789,7 +792,9 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0)
_airspeed_time(0),
_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_combined_time(0)
{}
void send(const hrt_abstime t)
@@ -799,12 +804,14 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@@ -813,7 +820,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
msg.alt = pos.alt;
msg.alt = sensor_combined.baro_alt_meter;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -879,8 +886,8 @@ protected:
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used;
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
@@ -950,11 +957,11 @@ protected:
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
msg.vx = pos.vel_n * 100.0f;
msg.vy = pos.vel_e * 100.0f;
msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
msg.vx = pos.vel_n * 100.0f;
msg.vy = pos.vel_e * 100.0f;
msg.vz = pos.vel_d * 100.0f;
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
}
@@ -1015,9 +1022,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
msg.vx = pos.vx;
msg.vy = pos.vy;
msg.vz = pos.vz;
msg.vx = pos.vx;
msg.vy = pos.vy;
msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
@@ -1078,9 +1085,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
msg.roll = pos.roll;
msg.pitch = pos.pitch;
msg.yaw = pos.yaw;
msg.roll = pos.roll;
msg.pitch = pos.pitch;
msg.yaw = pos.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}
src/modules/mavlink/mavlink_mission.cpp
+12 -9
View File
@@ -58,6 +58,10 @@
#endif
static const int ERROR = -1;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
_verbose(false)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wprl)) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpr)) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
if (CHECK_SYSID_COMPID_MISSION(wp)) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (CHECK_SYSID_COMPID_MISSION(wpca)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */
src/modules/mavlink/mavlink_mission.h
-2
View File
@@ -126,8 +126,6 @@ private:
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);
src/modules/mavlink/mavlink_receiver.cpp
+70 -22
View File
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0),
_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_long(msg);
break;
case MAVLINK_MSG_ID_COMMAND_INT:
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_OPTICAL_FLOW:
handle_message_optical_flow(msg);
break;
@@ -169,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_request_data_stream(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
@@ -276,6 +279,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
{
/* command */
mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
warnx("terminated by remote command");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
_mavlink->_task_should_exit = true;
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
if (_cmd_pub < 0) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
}
}
}
void
MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
{
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
}
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
tstatus.timestamp = tnow;
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -110,6 +110,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;
src/modules/navigator/navigator_main.cpp
+2 -2
View File
@@ -353,6 +353,8 @@ Navigator::task_main()
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
@@ -368,8 +370,6 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
_navigation_mode = &_offboard;
break;
src/modules/px4iofirmware/registers.c
+3 -1
View File
@@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
if (*values != PWM_IGNORE_THIS_CHANNEL) {
r_page_servos[offset] = *values;
}
offset++;
num_values--;
src/modules/sdlog2/sdlog2.c
+21
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
@@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct vision_position_estimate vision_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_EST1_s log_EST1;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_VISN_s log_VISN;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
@@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int vision_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- VISION POSITION --- */
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
log_msg.msg_type = LOG_VISN_MSG;
log_msg.body.log_VISN.x = buf.vision_pos.x;
log_msg.body.log_VISN.y = buf.vision_pos.y;
log_msg.body.log_VISN.z = buf.vision_pos.z;
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
LOGBUFFER_WRITE_AND_COUNT(VISN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
src/modules/sdlog2/sdlog2_messages.h
+15
View File
@@ -391,6 +391,20 @@ struct log_TEL_s {
uint64_t heartbeat_time;
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 38
struct log_VISN_s {
float x;
float y;
float z;
float vx;
float vy;
float vz;
float qx;
float qy;
float qz;
float qw;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
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"),
src/modules/sensors/sensor_params.c
+10
View File
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
/**
* QNH for barometer
*
* @min 500
* @max 1500
* @group Sensor Calibration
* @unit hPa
*/
PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
/**
* Board rotation
src/modules/sensors/sensors.cpp
+46 -21
View File
@@ -143,6 +143,12 @@
#define STICK_ON_OFF_LIMIT 0.75f
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/**
* Sensor app start / stop handling function
*
@@ -235,7 +241,7 @@ private:
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -258,7 +264,7 @@ private:
int board_rotation;
int external_mag_rotation;
float board_offset[3];
int rc_map_roll;
@@ -301,6 +307,8 @@ private:
float battery_voltage_scaling;
float battery_current_scaling;
float baro_qnh;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -354,9 +362,11 @@ private:
param_t board_rotation;
param_t external_mag_rotation;
param_t board_offset[3];
param_t baro_qnh;
} _parameter_handles; /**< handles for interesting parameters */
@@ -462,12 +472,6 @@ private:
namespace sensors
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Sensors *g_sensors = nullptr;
}
@@ -611,12 +615,15 @@ Sensors::Sensors() :
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
/* fetch initial parameter values */
parameters_update();
}
@@ -828,19 +835,37 @@ Sensors::parameters_update()
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
/** fine tune board offset on parameter update **/
math::Matrix<3, 3> board_rotation_offset;
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = _board_rotation * board_rotation_offset;
/* update barometer qnh setting */
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
int fd;
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
errx(1, "FATAL: no barometer found");
} else {
int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (ret) {
warnx("qnh could not be set");
close(fd);
return ERROR;
}
close(fd);
}
return OK;
}
@@ -1204,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
/* don't risk to feed negative airspeed into the system */
_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
@@ -1432,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
/* announce the airspeed if needed, just publish else */
src/modules/uORB/topics/differential_pressure.h
-1
View File
@@ -54,7 +54,6 @@
struct differential_pressure_s {
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
src/modules/uORB/topics/vehicle_command.h
+12 -7
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
/**
* @file vehicle_command.h
* Definition of the vehicle command uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -52,6 +53,9 @@
* but can contain additional ones.
*/
enum VEHICLE_CMD {
VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
@@ -87,7 +91,8 @@ enum VEHICLE_CMD {
VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */
VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */
VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */
VEHICLE_CMD_ENUM_END = 501, /* | */
VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
};
/**
@@ -115,8 +120,8 @@ struct vehicle_command_s {
float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */
float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */
float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */
float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */
enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
uint8_t target_system; /**< System which should execute the command */
src/modules/uavcan/esc_controller.cpp → src/modules/uavcan/actuators/esc.cpp
+2 -2
View File
@@ -32,12 +32,12 @@
****************************************************************************/
/**
* @file esc_controller.cpp
* @file esc.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include "esc.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :
src/modules/uavcan/esc_controller.hpp → src/modules/uavcan/actuators/esc.hpp
+1 -1
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file esc_controller.hpp
* @file esc.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand
src/modules/uavcan/module.mk
+13 -4
View File
@@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
# Main
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
uavcan_params.c
# Actuators
SRCS += actuators/esc.cpp
# Sensors
SRCS += sensors/sensor_bridge.cpp \
sensors/gnss.cpp \
sensors/mag.cpp \
sensors/baro.cpp
#
# libuavcan
src/modules/uavcan/sensors/baro.cpp
+117
View File
@@ -0,0 +1,117 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "baro.hpp"
#include <cmath>
static const orb_id_t BARO_TOPICS[2] = {
ORB_ID(sensor_baro0),
ORB_ID(sensor_baro1)
};
const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
_sub_air_data(node)
{
}
int UavcanBarometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case BAROIOCSMSLPRESSURE: {
if ((arg < 80000) || (arg > 120000)) {
return -EINVAL;
} else {
log("new msl pressure %u", _msl_pressure);
_msl_pressure = arg;
return OK;
}
}
case BAROIOCGMSLPRESSURE: {
return _msl_pressure;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
{
auto report = ::baro_report();
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
report.timestamp = msg.getMonotonicTimestamp().toUSec();
}
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
/*
* Altitude computation
* Refer to the MS5611 driver for details
*/
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
const double g = 9.80665; // gravity constant in m/s/s
const double R = 287.05; // ideal gas constant in J/kg/K
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
publish(msg.getSrcNodeID().get(), &report);
}
src/modules/uavcan/sensors/baro.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_baro.h>
#include <uavcan/equipment/air_data/StaticAirData.hpp>
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanBarometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
typedef uavcan::MethodBinder<UavcanBarometerBridge*,
void (UavcanBarometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)>
AirDataCbBinder;
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
unsigned _msl_pressure = 101325;
};
src/modules/uavcan/gnss_receiver.cpp → src/modules/uavcan/sensors/gnss.cpp
+62 -40
View File
@@ -32,52 +32,74 @@
****************************************************************************/
/**
* @file gnss_receiver.cpp
* @file gnss.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include "gnss.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_sub_fix(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
int UavcanGnssBridge::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
return res;
}
// Clear the uORB GPS report
memset(&_report, 0, sizeof(_report));
return res;
}
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
unsigned UavcanGnssBridge::get_num_redundant_channels() const
{
_report.timestamp_position = hrt_absolute_time();
_report.lat = msg.lat_1e7;
_report.lon = msg.lon_1e7;
_report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
return (_receiver_node_id < 0) ? 0 : 1;
}
_report.timestamp_variance = _report.timestamp_position;
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);
}
}
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
// This bridge does not support redundant GNSS receivers yet.
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.
}
}
auto report = ::vehicle_gps_position_s();
report.timestamp_position = hrt_absolute_time();
report.lat = msg.lat_1e7;
report.lon = msg.lon_1e7;
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
report.timestamp_variance = report.timestamp_position;
// Check if the msg contains valid covariance information
@@ -90,19 +112,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// Vertical position uncertainty
_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
_report.eph = -1.0F;
_report.epv = -1.0F;
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
* Use Jacobian to transform velocity covariance to heading covariance
@@ -118,36 +140,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
float vel_e = msg.ned_velocity[1];
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
_report.c_variance_rad =
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
_report.s_variance_m_s = -1.0F;
_report.c_variance_rad = -1.0F;
report.s_variance_m_s = -1.0F;
report.c_variance_rad = -1.0F;
}
_report.fix_type = msg.status;
report.fix_type = msg.status;
_report.timestamp_velocity = _report.timestamp_position;
_report.vel_n_m_s = msg.ned_velocity[0];
_report.vel_e_m_s = msg.ned_velocity[1];
_report.vel_d_m_s = msg.ned_velocity[2];
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
_report.vel_ned_valid = true;
report.timestamp_velocity = report.timestamp_position;
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
_report.timestamp_time = _report.timestamp_position;
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
report.timestamp_time = report.timestamp_position;
report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
_report.satellites_used = msg.sats_used;
report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report);
}
}
src/modules/uavcan/gnss_receiver.hpp → src/modules/uavcan/sensors/gnss.hpp
+20 -17
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gnss_receiver.hpp
* @file gnss.hpp
*
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
@@ -51,12 +51,22 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
#include "sensor_bridge.hpp"
class UavcanGnssBridge : public IUavcanSensorBridge
{
public:
UavcanGnssReceiver(uavcan::INode& node);
static const char *const NAME;
int init();
UavcanGnssBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
unsigned get_num_redundant_channels() const override;
void print_status() const override;
private:
/**
@@ -64,21 +74,14 @@ private:
*/
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
typedef uavcan::MethodBinder<UavcanGnssBridge*,
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
int _receiver_node_id = -1;
/*
* uORB
*/
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
};
src/modules/uavcan/sensors/mag.cpp
+123
View File
@@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "mag.hpp"
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
ORB_ID(sensor_mag2)
};
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
}
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) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case MAGIOCSSCALE: {
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;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
case MAGIOCGRANGE:
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
{
auto report = ::mag_report();
report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
report.timestamp = msg.getUtcTimestamp().toUSec();
if (report.timestamp == 0) {
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;
publish(msg.getSrcNodeID().get(), &report);
}
src/modules/uavcan/sensors/mag.hpp
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
public:
static const char *const NAME;
UavcanMagnetometerBridge(uavcan::INode& node);
const char *get_name() const override { return NAME; }
int init() override;
private:
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
typedef uavcan::MethodBinder<UavcanMagnetometerBridge*,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)>
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
mag_scale _scale = {};
};
src/modules/uavcan/sensors/sensor_bridge.cpp
+158
View File
@@ -0,0 +1,158 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "sensor_bridge.hpp"
#include <cassert>
#include "gnss.hpp"
#include "mag.hpp"
#include "baro.hpp"
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
list.add(new UavcanGnssBridge(node));
}
/*
* UavcanCDevSensorBridgeBase
*/
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
{
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
{
assert(report != nullptr);
Channel *channel = nullptr;
// Checking if such channel already exists
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id == node_id) {
channel = _channels + i;
break;
}
}
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return; // Give up immediately - saves some CPU time
}
log("adding channel %d...", node_id);
// Search for the first free channel
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id < 0) {
channel = _channels + i;
break;
}
}
// No free channels left
if (channel == nullptr) {
_out_of_channels = true;
log("out of channels");
return;
}
// 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;
log("out of class instances");
(void)unregister_class_devname(_class_devname, class_instance);
return;
}
// Publish to the appropriate topic, abort on failure
channel->orb_id = _orb_topics[class_instance];
channel->node_id = node_id;
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
*channel = Channel();
return;
}
log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(channel->orb_id, channel->orb_advert, 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;
}
void UavcanCDevSensorBridgeBase::print_status() const
{
printf("devname: %s\n", _class_devname);
for (unsigned i = 0; i < _max_channels; i++) {
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
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <containers/List.hpp>
#include <uavcan/uavcan.hpp>
#include <drivers/device/device.h>
#include <drivers/drv_orb_dev.h>
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
virtual ~IUavcanSensorBridge() { }
/**
* Returns ASCII name of the bridge.
*/
virtual const char *get_name() const = 0;
/**
* Starts the bridge.
* @return Non-negative value on success, negative on error.
*/
virtual int init() = 0;
/**
* Returns number of active redundancy channels.
*/
virtual unsigned get_num_redundant_channels() const = 0;
/**
* Prints current status in a human readable format to stdout.
*/
virtual void print_status() const = 0;
/**
* Sensor bridge factory.
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
};
/**
* This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
* For example, sensor_mag0, sensor_mag1, etc.
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
int node_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
};
const unsigned _max_channels;
const char *const _class_devname;
orb_id_t *const _orb_topics;
Channel *const _channels;
bool _out_of_channels = false;
protected:
template <unsigned MaxChannels>
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t (&orb_topics)[MaxChannels]) :
device::CDev(name, devname),
_max_channels(MaxChannels),
_class_devname(class_devname),
_orb_topics(new orb_id_t[MaxChannels]),
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
}
/**
* Sends one measurement into appropriate ORB topic.
* New redundancy channels will be registered automatically.
* @param node_id Sensor's Node ID
* @param report Pointer to ORB message object
*/
void publish(const int node_id, const void *report);
public:
virtual ~UavcanCDevSensorBridgeBase();
unsigned get_num_redundant_channels() const override;
void print_status() const override;
};
src/modules/uavcan/uavcan_clock.cpp
+2
View File
@@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment)
(void)adjustment;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt();
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return 0;
src/modules/uavcan/uavcan_main.cpp
+77 -58
View File
@@ -38,8 +38,10 @@
#include <fcntl.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 <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
@@ -65,16 +67,18 @@ UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_esc_controller(_node),
_gnss_receiver(_node)
_node_mutex(),
_esc_controller(_node)
{
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
// memset(_controls, 0, sizeof(_controls));
// memset(_poll_fds, 0, sizeof(_poll_fds));
const int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
}
UavcanNode::~UavcanNode()
@@ -99,10 +103,18 @@ UavcanNode::~UavcanNode()
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
// Removing the sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
auto next = br->getSibling();
delete br;
br = next;
}
_instance = nullptr;
}
@@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
@@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
// Do regular cdev init
ret = CDev::init();
if (ret != OK)
if (ret != OK) {
return ret;
}
_node.setName("org.pixhawk.pixhawk");
@@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id)
fill_node_info();
/* initializing the bridges UAVCAN <--> uORB */
// Actuators
ret = _esc_controller.init();
if (ret < 0)
if (ret < 0) {
return ret;
}
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
ret = br->init();
if (ret < 0) {
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
return ret;
}
warnx("sensor bridge '%s' init ok", br->get_name());
br = br->getSibling();
}
return _node.start();
}
@@ -248,6 +271,8 @@ void UavcanNode::node_spin_once()
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
@@ -291,8 +316,13 @@ int UavcanNode::run()
_groups_subscribed = _groups_required;
}
// Mutex is unlocked while the thread is blocked on IO multiplexing
(void)pthread_mutex_unlock(&_node_mutex);
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
(void)pthread_mutex_lock(&_node_mutex);
node_spin_once(); // Non-blocking
// this would be bad...
@@ -352,7 +382,6 @@ int UavcanNode::run()
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
@@ -376,10 +405,7 @@ int UavcanNode::run()
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@@ -520,8 +546,23 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
(void)pthread_mutex_lock(&_node_mutex);
// ESC mixer status
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
// Sensor bridges
auto br = _sensor_bridges.getHead();
while (br != nullptr) {
printf("Sensor '%s':\n", br->get_name());
br->print_status();
printf("\n");
br = br->getSibling();
}
(void)pthread_mutex_unlock(&_node_mutex);
}
/*
@@ -529,79 +570,57 @@ UavcanNode::print_info()
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
warnx("usage: \n"
"\tuavcan {start|status|stop}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
{
constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
if (argc < 3) {
print_usage();
::exit(1);
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Node ID
*/
const int node_id = atoi(argv[2]);
// Node ID
int32_t node_id = 0;
(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
/*
* CAN bitrate
*/
unsigned bitrate = 0;
// CAN bitrate
int32_t bitrate = 0;
(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
if (argc > 3) {
bitrate = atol(argv[3]);
}
if (bitrate <= 0) {
bitrate = DEFAULT_CAN_BITRATE;
}
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Start
*/
// Start
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *inst = UavcanNode::instance();
UavcanNode *const inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
return OK;
inst->print_info();
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
delete inst;
::exit(0);
}
print_usage();
src/modules/uavcan/uavcan_main.hpp
+9 -8
View File
@@ -42,8 +42,8 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
/**
* @file uavcan_main.hpp
@@ -77,12 +77,10 @@ public:
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
Node& get_node() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
@@ -109,8 +107,11 @@ private:
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
src/modules/uavcan/uavcan_params.c
+73
View File
@@ -0,0 +1,73 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
*
* @min 0
* @max 1
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
/**
* UAVCAN Node ID.
*
* Read the specs at http://uavcan.org to learn more about Node ID.
*
* @min 1
* @max 125
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
/**
* UAVCAN CAN bus bitrate.
*
* @min 20000
* @max 1000000
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);