2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
|
|
|
|
2016-05-27 10:04:55 -03:00
|
|
|
#include "GCS_Mavlink.h"
|
|
|
|
|
2017-08-08 02:58:52 -03:00
|
|
|
#include <AP_RangeFinder/RangeFinder_Backend.h>
|
|
|
|
|
2018-03-22 06:21:07 -03:00
|
|
|
MAV_TYPE GCS_MAVLINK_Rover::frame_type() const
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2018-03-22 06:21:07 -03:00
|
|
|
if (rover.is_boat()) {
|
|
|
|
return MAV_TYPE_SURFACE_BOAT;
|
2013-02-08 22:11:43 -04:00
|
|
|
}
|
2018-03-22 06:21:07 -03:00
|
|
|
return MAV_TYPE_GROUND_ROVER;
|
|
|
|
}
|
|
|
|
|
|
|
|
MAV_MODE GCS_MAVLINK_Rover::base_mode() const
|
|
|
|
{
|
2018-04-03 11:35:23 -03:00
|
|
|
uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
// work out the base_mode. This value is not very useful
|
|
|
|
// for APM, but we calculate it as best we can so a generic
|
|
|
|
// MAVLink enabled ground station can work out something about
|
|
|
|
// what the MAV is up to. The actual bit values are highly
|
|
|
|
// ambiguous for most of the APM flight modes. In practice, you
|
|
|
|
// only get useful information from the custom_mode, which maps to
|
|
|
|
// the APM flight mode and has a well defined meaning in the
|
|
|
|
// ArduPlane documentation
|
2018-03-22 06:21:07 -03:00
|
|
|
if (rover.control_mode->has_manual_input()) {
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2018-03-22 06:21:07 -03:00
|
|
|
if (rover.control_mode->is_autopilot_mode()) {
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
|
2017-07-18 23:19:08 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED) // TODO ???? Remove !
|
|
|
|
if (control_mode->stick_mixing_enabled()) {
|
2012-04-30 04:17:14 -03:00
|
|
|
// all modes except INITIALISING have some form of manual
|
|
|
|
// override if stick mixing is enabled
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
2012-04-30 04:17:14 -03:00
|
|
|
#endif
|
2018-03-22 06:21:07 -03:00
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
// we are armed if we are not initialising
|
2018-03-22 06:21:07 -03:00
|
|
|
if (rover.control_mode != &rover.mode_initializing && rover.arming.is_armed()) {
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// indicate we have set a custom mode
|
2018-04-03 11:35:23 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2018-04-03 11:35:23 -03:00
|
|
|
return (MAV_MODE)_base_mode;
|
2018-03-22 06:21:07 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t GCS_MAVLINK_Rover::custom_mode() const
|
|
|
|
{
|
|
|
|
return rover.control_mode->mode_number();
|
|
|
|
}
|
|
|
|
|
|
|
|
MAV_STATE GCS_MAVLINK_Rover::system_status() const
|
|
|
|
{
|
2018-12-18 06:28:20 -04:00
|
|
|
if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
|
2018-03-22 06:21:07 -03:00
|
|
|
return MAV_STATE_CRITICAL;
|
|
|
|
}
|
|
|
|
if (rover.control_mode == &rover.mode_initializing) {
|
|
|
|
return MAV_STATE_CALIBRATING;
|
|
|
|
}
|
|
|
|
if (rover.control_mode == &rover.mode_hold) {
|
|
|
|
return MAV_STATE_STANDBY;
|
|
|
|
}
|
|
|
|
|
|
|
|
return MAV_STATE_ACTIVE;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2019-01-22 21:36:59 -04:00
|
|
|
void GCS_MAVLINK_Rover::get_sensor_status_flags(uint32_t &present,
|
|
|
|
uint32_t &enabled,
|
|
|
|
uint32_t &health)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2019-01-22 21:36:59 -04:00
|
|
|
rover.update_sensor_status_flags();
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2019-01-22 21:36:59 -04:00
|
|
|
present = rover.control_sensors_present;
|
|
|
|
enabled = rover.control_sensors_enabled;
|
|
|
|
health = rover.control_sensors_health;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2015-05-12 04:00:25 -03:00
|
|
|
void Rover::send_nav_controller_output(mavlink_channel_t chan)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
|
|
|
mavlink_msg_nav_controller_output_send(
|
|
|
|
chan,
|
2018-12-11 09:32:14 -04:00
|
|
|
0, // roll
|
|
|
|
degrees(g2.attitude_control.get_desired_pitch()),
|
2018-12-11 10:04:50 -04:00
|
|
|
control_mode->nav_bearing(),
|
|
|
|
control_mode->wp_bearing(),
|
2017-08-01 20:14:49 -03:00
|
|
|
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
|
2013-09-08 21:18:31 -03:00
|
|
|
0,
|
2017-08-01 20:14:49 -03:00
|
|
|
control_mode->speed_error(),
|
2018-12-11 10:04:50 -04:00
|
|
|
control_mode->crosstrack_error());
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2015-05-12 04:00:25 -03:00
|
|
|
void Rover::send_servo_out(mavlink_channel_t chan)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2017-11-20 19:37:42 -04:00
|
|
|
float motor1, motor3;
|
|
|
|
if (g2.motors.have_skid_steering()) {
|
|
|
|
motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleLeft) / 1000.0f);
|
|
|
|
motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleRight) / 1000.0f);
|
|
|
|
} else {
|
|
|
|
motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f);
|
|
|
|
motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f);
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
mavlink_msg_rc_channels_scaled_send(
|
|
|
|
chan,
|
|
|
|
millis(),
|
2016-11-23 13:13:56 -04:00
|
|
|
0, // port 0
|
2017-11-20 19:37:42 -04:00
|
|
|
motor1,
|
2013-02-07 18:21:22 -04:00
|
|
|
0,
|
2017-11-20 19:37:42 -04:00
|
|
|
motor3,
|
2013-02-07 18:21:22 -04:00
|
|
|
0,
|
2012-04-30 04:17:14 -03:00
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
2018-05-02 07:33:43 -03:00
|
|
|
rssi.read_receiver_rssi_uint8());
|
2015-02-05 02:23:49 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2018-01-30 22:11:27 -04:00
|
|
|
int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2018-01-30 22:11:27 -04:00
|
|
|
return rover.g2.motors.get_throttle();
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2018-12-18 06:47:19 -04:00
|
|
|
void GCS_MAVLINK_Rover::send_rangefinder() const
|
2013-02-28 21:00:48 -04:00
|
|
|
{
|
2017-08-08 02:58:52 -03:00
|
|
|
float distance_cm;
|
|
|
|
float voltage;
|
|
|
|
bool got_one = false;
|
|
|
|
|
|
|
|
// report smaller distance of all rangefinders
|
2018-12-18 06:47:19 -04:00
|
|
|
for (uint8_t i=0; i<rover.rangefinder.num_sensors(); i++) {
|
|
|
|
AP_RangeFinder_Backend *s = rover.rangefinder.get_backend(i);
|
2017-08-08 02:58:52 -03:00
|
|
|
if (s == nullptr) {
|
|
|
|
continue;
|
2015-04-17 03:41:00 -03:00
|
|
|
}
|
2017-08-08 02:58:52 -03:00
|
|
|
if (!got_one ||
|
|
|
|
s->distance_cm() < distance_cm) {
|
|
|
|
distance_cm = s->distance_cm();
|
|
|
|
voltage = s->voltage_mv();
|
|
|
|
got_one = true;
|
2013-03-28 21:00:41 -03:00
|
|
|
}
|
|
|
|
}
|
2017-08-08 02:58:52 -03:00
|
|
|
if (!got_one) {
|
|
|
|
// no relevant data found
|
|
|
|
return;
|
|
|
|
}
|
2015-04-17 03:41:00 -03:00
|
|
|
|
2013-02-28 21:00:48 -04:00
|
|
|
mavlink_msg_rangefinder_send(
|
|
|
|
chan,
|
2013-03-28 21:00:41 -03:00
|
|
|
distance_cm * 0.01f,
|
|
|
|
voltage);
|
2013-02-28 21:00:48 -04:00
|
|
|
}
|
|
|
|
|
2018-12-30 16:43:39 -04:00
|
|
|
/*
|
|
|
|
send RPM packet
|
|
|
|
*/
|
|
|
|
void Rover::send_rpm(mavlink_channel_t chan)
|
|
|
|
{
|
|
|
|
if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
|
|
|
|
mavlink_msg_rpm_send(
|
|
|
|
chan,
|
|
|
|
rpm_sensor.get_rpm(0),
|
|
|
|
rpm_sensor.get_rpm(1));
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-06-18 04:37:59 -03:00
|
|
|
/*
|
|
|
|
send PID tuning message
|
|
|
|
*/
|
|
|
|
void Rover::send_pid_tuning(mavlink_channel_t chan)
|
|
|
|
{
|
2019-01-18 00:23:42 -04:00
|
|
|
const AP_Logger::PID_Info *pid_info;
|
2018-01-01 02:27:39 -04:00
|
|
|
// steering PID
|
|
|
|
if (g.gcs_pid_mask & 1) {
|
2017-08-08 02:37:21 -03:00
|
|
|
pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
|
2016-11-23 13:13:56 -04:00
|
|
|
mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
|
2017-10-25 09:17:51 -03:00
|
|
|
degrees(pid_info->desired),
|
|
|
|
degrees(ahrs.get_yaw_rate_earth()),
|
2016-05-29 20:44:23 -03:00
|
|
|
pid_info->FF,
|
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
2018-08-04 00:24:03 -03:00
|
|
|
|
2018-01-01 02:27:39 -04:00
|
|
|
// speed to throttle PID
|
|
|
|
if (g.gcs_pid_mask & 2) {
|
2017-08-08 21:24:30 -03:00
|
|
|
pid_info = &g2.attitude_control.get_throttle_speed_pid().get_pid_info();
|
2017-10-25 09:17:51 -03:00
|
|
|
float speed = 0.0f;
|
|
|
|
g2.attitude_control.get_forward_speed(speed);
|
2016-11-23 13:13:56 -04:00
|
|
|
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
|
2016-05-29 20:44:23 -03:00
|
|
|
pid_info->desired,
|
2017-10-25 09:17:51 -03:00
|
|
|
speed,
|
2016-05-29 20:44:23 -03:00
|
|
|
0,
|
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
2015-06-18 04:37:59 -03:00
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
2018-06-05 05:56:46 -03:00
|
|
|
|
|
|
|
// pitch to throttle pid
|
|
|
|
if (g.gcs_pid_mask & 4) {
|
|
|
|
pid_info = &g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info();
|
|
|
|
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
|
2018-08-08 01:14:40 -03:00
|
|
|
degrees(pid_info->desired),
|
|
|
|
degrees(ahrs.pitch),
|
|
|
|
pid_info->FF,
|
2018-08-08 01:15:07 -03:00
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// left wheel rate control pid
|
|
|
|
if (g.gcs_pid_mask & 8) {
|
|
|
|
pid_info = &g2.wheel_rate_control.get_pid(0).get_pid_info();
|
|
|
|
mavlink_msg_pid_tuning_send(chan, 7,
|
|
|
|
pid_info->desired,
|
|
|
|
pid_info->actual,
|
|
|
|
pid_info->FF,
|
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// right wheel rate control pid
|
|
|
|
if (g.gcs_pid_mask & 16) {
|
|
|
|
pid_info = &g2.wheel_rate_control.get_pid(1).get_pid_info();
|
|
|
|
mavlink_msg_pid_tuning_send(chan, 8,
|
|
|
|
pid_info->desired,
|
|
|
|
pid_info->actual,
|
|
|
|
pid_info->FF,
|
2018-06-05 05:56:46 -03:00
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
2018-09-25 10:09:47 -03:00
|
|
|
|
|
|
|
// sailboat heel to mainsail pid
|
|
|
|
if (g.gcs_pid_mask & 32) {
|
|
|
|
pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info();
|
|
|
|
mavlink_msg_pid_tuning_send(chan, 9,
|
|
|
|
pid_info->desired,
|
|
|
|
pid_info->actual,
|
|
|
|
pid_info->FF,
|
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
2015-06-18 04:37:59 -03:00
|
|
|
}
|
|
|
|
|
2019-01-25 22:14:00 -04:00
|
|
|
void Rover::send_wheel_encoder_distance(mavlink_channel_t chan)
|
|
|
|
{
|
|
|
|
// send wheel encoder data using wheel_distance message
|
|
|
|
if (g2.wheel_encoder.num_sensors() > 0) {
|
2019-01-28 06:06:50 -04:00
|
|
|
double distances[MAVLINK_MSG_WHEEL_DISTANCE_FIELD_DISTANCE_LEN] {};
|
2019-01-25 22:14:00 -04:00
|
|
|
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
|
|
|
|
distances[i] = wheel_encoder_last_distance_m[i];
|
|
|
|
}
|
|
|
|
mavlink_msg_wheel_distance_send(chan, 1000UL * wheel_encoder_last_ekf_update_ms, g2.wheel_encoder.num_sensors(), distances);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-07-12 04:42:48 -03:00
|
|
|
uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const
|
|
|
|
{
|
|
|
|
return rover.g.sysid_my_gcs;
|
|
|
|
}
|
2018-12-13 19:12:34 -04:00
|
|
|
bool GCS_MAVLINK_Rover::sysid_enforce() const
|
|
|
|
{
|
|
|
|
return rover.g2.sysid_enforce;
|
|
|
|
}
|
2017-07-12 04:42:48 -03:00
|
|
|
|
2016-05-29 21:08:46 -03:00
|
|
|
uint32_t GCS_MAVLINK_Rover::telem_delay() const
|
2012-08-29 20:36:18 -03:00
|
|
|
{
|
2017-03-30 11:07:24 -03:00
|
|
|
return static_cast<uint32_t>(rover.g.telem_delay);
|
2012-08-29 20:36:18 -03:00
|
|
|
}
|
|
|
|
|
2018-12-17 22:07:15 -04:00
|
|
|
bool GCS_MAVLINK_Rover::vehicle_initialised() const
|
|
|
|
{
|
|
|
|
return rover.control_mode != &rover.mode_initializing;
|
|
|
|
}
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
2016-05-27 10:04:55 -03:00
|
|
|
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2018-05-21 02:26:50 -03:00
|
|
|
// if we don't have at least 0.2ms remaining before the main loop
|
2013-10-27 20:33:52 -03:00
|
|
|
// wants to fire then don't send a mavlink message. We want to
|
|
|
|
// prioritise the main flight control loop over communications
|
2018-05-08 02:57:58 -03:00
|
|
|
if (!hal.scheduler->in_delay_callback() &&
|
2019-01-04 02:57:36 -04:00
|
|
|
!AP_BoardConfig::in_sensor_config_error() &&
|
2018-05-21 02:26:50 -03:00
|
|
|
rover.scheduler.time_available_usec() < 200) {
|
2017-08-20 23:32:34 -03:00
|
|
|
gcs().set_out_of_time(true);
|
2013-10-27 20:33:52 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
switch (id) {
|
|
|
|
|
|
|
|
case MSG_NAV_CONTROLLER_OUTPUT:
|
2018-12-11 10:04:50 -04:00
|
|
|
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
|
|
|
rover.send_nav_controller_output(chan);
|
2012-04-30 04:17:14 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case MSG_SERVO_OUT:
|
|
|
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
2015-05-12 04:00:25 -03:00
|
|
|
rover.send_servo_out(chan);
|
2012-04-30 04:17:14 -03:00
|
|
|
break;
|
|
|
|
|
2017-07-20 23:56:39 -03:00
|
|
|
case MSG_RPM:
|
|
|
|
CHECK_PAYLOAD_SIZE(RPM);
|
2018-12-30 16:43:39 -04:00
|
|
|
rover.send_rpm(chan);
|
2017-07-20 23:56:39 -03:00
|
|
|
break;
|
|
|
|
|
2019-01-25 22:14:00 -04:00
|
|
|
case MSG_WHEEL_DISTANCE:
|
|
|
|
CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
|
|
|
|
rover.send_wheel_encoder_distance(chan);
|
|
|
|
break;
|
|
|
|
|
2018-09-25 10:09:47 -03:00
|
|
|
case MSG_WIND:
|
|
|
|
CHECK_PAYLOAD_SIZE(WIND);
|
2018-11-01 02:27:25 -03:00
|
|
|
rover.g2.windvane.send_wind(chan);
|
2015-01-06 00:17:56 -04:00
|
|
|
break;
|
2015-01-06 00:28:38 -04:00
|
|
|
|
2015-06-18 04:37:59 -03:00
|
|
|
case MSG_PID_TUNING:
|
|
|
|
CHECK_PAYLOAD_SIZE(PID_TUNING);
|
|
|
|
rover.send_pid_tuning(chan);
|
|
|
|
break;
|
|
|
|
|
2017-07-18 03:43:57 -03:00
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::try_send_message(id);
|
2015-07-30 22:24:32 -03:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-07-07 02:12:45 -03:00
|
|
|
void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, mavlink_message_t &msg)
|
|
|
|
{
|
|
|
|
// pass message to follow library
|
|
|
|
rover.g2.follow.handle_msg(msg);
|
|
|
|
GCS_MAVLINK::packetReceived(status, msg);
|
|
|
|
}
|
|
|
|
|
2013-10-20 19:32:39 -03:00
|
|
|
/*
|
|
|
|
default stream rates to 1Hz
|
|
|
|
*/
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Param: RAW_SENS
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Raw sensor stream rate
|
|
|
|
// @Description: Raw sensor stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRates[0], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: EXT_STAT
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Extended status stream rate to ground station
|
|
|
|
// @Description: Extended status stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRates[1], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: RC_CHAN
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: RC Channel stream rate to ground station
|
|
|
|
// @Description: RC Channel stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRates[2], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: RAW_CTRL
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Raw Control stream rate to ground station
|
|
|
|
// @Description: Raw Control stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRates[3], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: POSITION
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Position stream rate to ground station
|
|
|
|
// @Description: Position stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRates[4], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA1
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Extra data type 1 stream rate to ground station
|
|
|
|
// @Description: Extra data type 1 stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRates[5], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA2
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Extra data type 2 stream rate to ground station
|
|
|
|
// @Description: Extra data type 2 stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRates[6], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA3
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Extra data type 3 stream rate to ground station
|
|
|
|
// @Description: Extra data type 3 stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-20 19:32:39 -03:00
|
|
|
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRates[7], 1),
|
2013-09-11 20:51:36 -03:00
|
|
|
|
|
|
|
// @Param: PARAMS
|
2013-10-20 19:32:39 -03:00
|
|
|
// @DisplayName: Parameter stream rate to ground station
|
|
|
|
// @Description: Parameter stream rate to ground station
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Units: Hz
|
2013-10-20 19:32:39 -03:00
|
|
|
// @Range: 0 10
|
2013-09-11 20:51:36 -03:00
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2013-10-27 20:33:52 -03:00
|
|
|
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10),
|
2012-04-30 04:17:14 -03:00
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_RAW_SENSORS_msgs[] = {
|
2018-12-04 20:36:50 -04:00
|
|
|
MSG_RAW_IMU,
|
|
|
|
MSG_SCALED_IMU2,
|
|
|
|
MSG_SCALED_IMU3,
|
2018-12-07 02:29:16 -04:00
|
|
|
MSG_SCALED_PRESSURE,
|
|
|
|
MSG_SCALED_PRESSURE2,
|
|
|
|
MSG_SCALED_PRESSURE3,
|
2018-12-04 01:54:00 -04:00
|
|
|
MSG_SENSOR_OFFSETS
|
2017-08-21 02:53:37 -03:00
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
|
2018-12-17 22:07:15 -04:00
|
|
|
MSG_SYS_STATUS,
|
|
|
|
MSG_POWER_STATUS,
|
2018-12-04 01:18:21 -04:00
|
|
|
MSG_MEMINFO,
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_CURRENT_WAYPOINT,
|
|
|
|
MSG_GPS_RAW,
|
|
|
|
MSG_GPS_RTK,
|
|
|
|
MSG_GPS2_RAW,
|
|
|
|
MSG_GPS2_RTK,
|
|
|
|
MSG_NAV_CONTROLLER_OUTPUT,
|
|
|
|
MSG_FENCE_STATUS,
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_POSITION_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_LOCATION,
|
|
|
|
MSG_LOCAL_POSITION
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_SERVO_OUT,
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_SERVO_OUTPUT_RAW,
|
|
|
|
MSG_RADIO_IN
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_EXTRA1_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_ATTITUDE,
|
2018-12-18 16:22:59 -04:00
|
|
|
MSG_SIMSTATE,
|
|
|
|
MSG_AHRS2,
|
|
|
|
MSG_AHRS3,
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_PID_TUNING,
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_EXTRA2_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_VFR_HUD
|
|
|
|
};
|
2018-05-21 01:06:33 -03:00
|
|
|
static const ap_message STREAM_EXTRA3_msgs[] = {
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_AHRS,
|
|
|
|
MSG_HWSTATUS,
|
2018-09-25 10:09:47 -03:00
|
|
|
MSG_WIND,
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_RANGEFINDER,
|
2018-12-18 06:47:19 -04:00
|
|
|
MSG_DISTANCE_SENSOR,
|
2017-08-21 02:53:37 -03:00
|
|
|
MSG_SYSTEM_TIME,
|
|
|
|
MSG_BATTERY2,
|
|
|
|
MSG_BATTERY_STATUS,
|
|
|
|
MSG_MOUNT_STATUS,
|
|
|
|
MSG_MAG_CAL_REPORT,
|
|
|
|
MSG_MAG_CAL_PROGRESS,
|
|
|
|
MSG_EKF_STATUS_REPORT,
|
|
|
|
MSG_VIBRATION,
|
2018-06-20 20:51:06 -03:00
|
|
|
MSG_RPM,
|
2019-01-25 22:14:00 -04:00
|
|
|
MSG_WHEEL_DISTANCE,
|
2018-06-20 20:51:06 -03:00
|
|
|
MSG_ESC_TELEMETRY,
|
2017-08-21 02:53:37 -03:00
|
|
|
};
|
2018-05-22 22:23:12 -03:00
|
|
|
static const ap_message STREAM_PARAMS_msgs[] = {
|
|
|
|
MSG_NEXT_PARAM
|
|
|
|
};
|
2012-12-18 15:30:42 -04:00
|
|
|
|
2017-08-21 02:53:37 -03:00
|
|
|
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_POSITION),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA1),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA2),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA3),
|
2018-05-22 22:23:12 -03:00
|
|
|
MAV_STREAM_ENTRY(STREAM_PARAMS),
|
2017-08-21 02:53:37 -03:00
|
|
|
MAV_STREAM_TERMINATOR // must have this at end of stream_entries
|
|
|
|
};
|
2013-10-27 20:33:52 -03:00
|
|
|
|
2017-08-21 02:53:37 -03:00
|
|
|
bool GCS_MAVLINK_Rover::in_hil_mode() const
|
|
|
|
{
|
2012-12-18 15:30:42 -04:00
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
2017-08-21 02:53:37 -03:00
|
|
|
return rover.g.hil_mode == 1;
|
2012-12-18 15:30:42 -04:00
|
|
|
#endif
|
2017-08-21 02:53:37 -03:00
|
|
|
return false;
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2016-05-27 10:04:55 -03:00
|
|
|
bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2017-07-18 23:19:08 -03:00
|
|
|
if (rover.control_mode != &rover.mode_guided) {
|
2015-06-03 09:31:31 -03:00
|
|
|
// only accept position updates when in GUIDED mode
|
2016-04-26 07:17:40 -03:00
|
|
|
return false;
|
2015-06-03 09:31:31 -03:00
|
|
|
}
|
2014-03-18 20:06:58 -03:00
|
|
|
|
|
|
|
// make any new wp uploaded instant (in case we are already in Guided mode)
|
2017-08-01 20:10:38 -03:00
|
|
|
rover.mode_guided.set_desired_location(cmd.content.location);
|
2016-04-26 07:17:40 -03:00
|
|
|
return true;
|
2014-03-18 20:06:58 -03:00
|
|
|
}
|
|
|
|
|
2016-05-27 10:04:55 -03:00
|
|
|
void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
2014-03-18 20:06:58 -03:00
|
|
|
{
|
|
|
|
// nothing to do
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2018-03-17 05:19:30 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
|
|
|
|
{
|
|
|
|
if (is_equal(packet.param4, 1.0f)) {
|
2018-05-25 17:26:55 -03:00
|
|
|
if (rover.trim_radio()) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-09-26 22:41:26 -03:00
|
|
|
} else if (is_equal(packet.param6, 1.0f)) {
|
|
|
|
if (rover.g2.windvane.start_calibration()) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-03-17 05:19:30 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
|
|
|
|
}
|
|
|
|
|
2018-07-05 22:34:29 -03:00
|
|
|
bool GCS_MAVLINK_Rover::set_home_to_current_location(bool lock) {
|
|
|
|
return rover.set_home_to_current_location(lock);
|
|
|
|
}
|
|
|
|
bool GCS_MAVLINK_Rover::set_home(const Location& loc, bool lock) {
|
|
|
|
return rover.set_home(loc, lock);
|
|
|
|
}
|
|
|
|
|
2018-07-03 23:38:22 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet)
|
|
|
|
{
|
|
|
|
switch (packet.command) {
|
|
|
|
|
|
|
|
case MAV_CMD_DO_CHANGE_SPEED:
|
|
|
|
// param1 : unused
|
|
|
|
// param2 : new speed in m/s
|
|
|
|
if (!rover.control_mode->set_desired_speed(packet.param2)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
2018-08-22 01:46:35 -03:00
|
|
|
case MAV_CMD_DO_SET_REVERSE:
|
|
|
|
// param1 : Direction (0=Forward, 1=Reverse)
|
|
|
|
rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
2018-07-03 23:38:22 -03:00
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::handle_command_int_packet(packet);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-03 23:32:04 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet)
|
|
|
|
{
|
|
|
|
switch (packet.command) {
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
2018-11-12 02:26:38 -04:00
|
|
|
if (rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
2018-07-03 23:32:04 -03:00
|
|
|
|
|
|
|
case MAV_CMD_MISSION_START:
|
2018-11-12 02:26:38 -04:00
|
|
|
if (rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
2018-07-03 23:32:04 -03:00
|
|
|
|
|
|
|
case MAV_CMD_COMPONENT_ARM_DISARM:
|
|
|
|
if (is_equal(packet.param1, 1.0f)) {
|
|
|
|
// run pre_arm_checks and arm_checks and display failures
|
|
|
|
if (rover.arm_motors(AP_Arming::MAVLINK)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
} else if (is_zero(packet.param1)) {
|
|
|
|
if (rover.disarm_motors()) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_CHANGE_SPEED:
|
|
|
|
// param1 : unused
|
|
|
|
// param2 : new speed in m/s
|
|
|
|
if (!rover.control_mode->set_desired_speed(packet.param2)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_SET_HOME:
|
|
|
|
{
|
|
|
|
// param1 : use current (1=use current location, 0=use specified location)
|
|
|
|
// param5 : latitude
|
|
|
|
// param6 : longitude
|
|
|
|
// param7 : altitude
|
|
|
|
if (is_equal(packet.param1, 1.0f)) {
|
|
|
|
if (rover.set_home_to_current_location(true)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// ensure param1 is zero
|
|
|
|
if (!is_zero(packet.param1)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
Location new_home_loc {};
|
|
|
|
new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f);
|
|
|
|
new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f);
|
|
|
|
new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f);
|
|
|
|
if (rover.set_home(new_home_loc, true)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
2018-08-22 01:46:35 -03:00
|
|
|
case MAV_CMD_DO_SET_REVERSE:
|
|
|
|
// param1 : Direction (0=Forward, 1=Reverse)
|
|
|
|
rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
2018-07-03 23:32:04 -03:00
|
|
|
case MAV_CMD_NAV_SET_YAW_SPEED:
|
|
|
|
{
|
|
|
|
// param1 : yaw angle to adjust direction by in centidegress
|
|
|
|
// param2 : Speed - normalized to 0 .. 1
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
|
|
|
if (rover.control_mode != &rover.mode_guided) {
|
2018-11-12 02:26:38 -04:00
|
|
|
return MAV_RESULT_FAILED;
|
2018-07-03 23:32:04 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// send yaw change and target speed to guided mode controller
|
|
|
|
const float speed_max = rover.control_mode->get_speed_default();
|
|
|
|
const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max);
|
|
|
|
rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed);
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAV_CMD_DO_MOTOR_TEST:
|
|
|
|
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
|
|
|
|
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
|
|
|
|
// param3 : throttle (range depends upon param2)
|
|
|
|
// param4 : timeout (in seconds)
|
|
|
|
return rover.mavlink_motor_test_start(chan,
|
|
|
|
static_cast<uint8_t>(packet.param1),
|
|
|
|
static_cast<uint8_t>(packet.param2),
|
|
|
|
static_cast<int16_t>(packet.param3),
|
|
|
|
packet.param4);
|
|
|
|
|
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::handle_command_long_packet(packet);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2019-02-03 17:40:30 -04:00
|
|
|
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
|
|
|
|
void GCS_MAVLINK_Rover::handle_rc_channels_override(const mavlink_message_t *msg)
|
|
|
|
{
|
|
|
|
rover.failsafe.last_heartbeat_ms = AP_HAL::millis();
|
|
|
|
GCS_MAVLINK::handle_rc_channels_override(msg);
|
|
|
|
}
|
|
|
|
|
2018-07-03 23:32:04 -03:00
|
|
|
|
2016-05-27 10:04:55 -03:00
|
|
|
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
2014-03-18 20:06:58 -03:00
|
|
|
{
|
2012-04-30 04:17:14 -03:00
|
|
|
switch (msg->msgid) {
|
2017-09-18 02:37:37 -03:00
|
|
|
|
2019-02-03 17:40:30 -04:00
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2017-04-27 13:03:24 -03:00
|
|
|
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
|
|
|
{
|
|
|
|
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
mavlink_manual_control_t packet;
|
|
|
|
mavlink_msg_manual_control_decode(msg, &packet);
|
2018-04-03 20:20:51 -03:00
|
|
|
|
|
|
|
if (packet.target != rover.g.sysid_this_mav) {
|
|
|
|
break; // only accept control aimed at us
|
|
|
|
}
|
2018-05-29 03:26:21 -03:00
|
|
|
|
|
|
|
uint32_t tnow = AP_HAL::millis();
|
2017-04-27 13:03:24 -03:00
|
|
|
|
|
|
|
const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f;
|
|
|
|
const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f;
|
2018-05-29 03:26:21 -03:00
|
|
|
RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow);
|
|
|
|
RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow);
|
2017-04-27 13:03:24 -03:00
|
|
|
|
2018-10-31 02:27:56 -03:00
|
|
|
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
|
|
|
|
rover.failsafe.last_heartbeat_ms = tnow;
|
2017-04-27 13:03:24 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
|
|
|
{
|
2018-10-31 02:27:56 -03:00
|
|
|
// we keep track of the last time we received a heartbeat from our GCS for failsafe purposes
|
2016-11-23 13:13:56 -04:00
|
|
|
if (msg->sysid != rover.g.sysid_my_gcs) {
|
|
|
|
break;
|
|
|
|
}
|
2018-10-31 02:24:22 -03:00
|
|
|
rover.failsafe.last_heartbeat_ms = AP_HAL::millis();
|
2012-04-30 04:17:14 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2017-05-03 11:21:58 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
|
|
|
|
{
|
|
|
|
// decode packet
|
|
|
|
mavlink_set_attitude_target_t packet;
|
|
|
|
mavlink_msg_set_attitude_target_decode(msg, &packet);
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
2017-07-18 23:19:08 -03:00
|
|
|
if (rover.control_mode != &rover.mode_guided) {
|
2017-05-03 11:21:58 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure type_mask specifies to use thrust
|
|
|
|
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// convert thrust to ground speed
|
2017-08-01 04:07:59 -03:00
|
|
|
packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
|
2017-12-05 21:41:28 -04:00
|
|
|
const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
|
2017-05-03 11:21:58 -03:00
|
|
|
|
2017-08-01 04:08:33 -03:00
|
|
|
// if the body_yaw_rate field is ignored, convert quaternion to heading
|
2017-05-03 11:21:58 -03:00
|
|
|
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
|
|
|
|
// convert quaternion to heading
|
2017-08-01 04:08:33 -03:00
|
|
|
float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
|
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
|
2017-05-03 11:21:58 -03:00
|
|
|
} else {
|
2017-08-01 20:10:38 -03:00
|
|
|
// use body_yaw_rate field
|
|
|
|
rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2016-08-23 03:04:30 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
|
|
|
|
{
|
|
|
|
// decode packet
|
|
|
|
mavlink_set_position_target_local_ned_t packet;
|
|
|
|
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
2017-07-18 23:19:08 -03:00
|
|
|
if (rover.control_mode != &rover.mode_guided) {
|
2016-08-23 03:04:30 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check for supported coordinate frames
|
|
|
|
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
|
|
|
|
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
|
|
|
|
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
|
|
|
|
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
|
2017-05-03 11:21:58 -03:00
|
|
|
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
|
|
|
|
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
|
2017-08-01 07:09:05 -03:00
|
|
|
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
|
|
|
|
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
|
2017-05-03 11:21:58 -03:00
|
|
|
|
2017-08-01 07:09:05 -03:00
|
|
|
// prepare target position
|
2017-05-03 11:21:58 -03:00
|
|
|
Location target_loc = rover.current_loc;
|
2016-08-23 03:04:30 -03:00
|
|
|
if (!pos_ignore) {
|
2016-12-23 05:49:39 -04:00
|
|
|
switch (packet.coordinate_frame) {
|
|
|
|
case MAV_FRAME_BODY_NED:
|
|
|
|
case MAV_FRAME_BODY_OFFSET_NED: {
|
2016-08-23 03:04:30 -03:00
|
|
|
// rotate from body-frame to NE frame
|
2017-05-03 11:06:54 -03:00
|
|
|
const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
|
|
|
|
const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
|
2016-08-23 03:04:30 -03:00
|
|
|
// add offset to current location
|
2017-05-03 11:21:58 -03:00
|
|
|
location_offset(target_loc, ne_x, ne_y);
|
2016-12-23 05:49:39 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case MAV_FRAME_LOCAL_OFFSET_NED:
|
2016-08-23 03:04:30 -03:00
|
|
|
// add offset to current location
|
2017-05-03 11:21:58 -03:00
|
|
|
location_offset(target_loc, packet.x, packet.y);
|
2016-12-23 05:49:39 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
2016-08-23 03:04:30 -03:00
|
|
|
// MAV_FRAME_LOCAL_NED interpret as an offset from home
|
2017-05-03 11:21:58 -03:00
|
|
|
target_loc = rover.ahrs.get_home();
|
|
|
|
location_offset(target_loc, packet.x, packet.y);
|
2016-12-23 05:49:39 -04:00
|
|
|
break;
|
2016-08-23 03:04:30 -03:00
|
|
|
}
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
|
2017-05-03 11:21:58 -03:00
|
|
|
float target_speed = 0.0f;
|
2017-08-01 07:09:05 -03:00
|
|
|
float target_yaw_cd = 0.0f;
|
|
|
|
|
|
|
|
// consume velocity and convert to target speed and heading
|
2017-05-03 11:21:58 -03:00
|
|
|
if (!vel_ignore) {
|
2017-12-05 21:41:28 -04:00
|
|
|
const float speed_max = rover.control_mode->get_speed_default();
|
2017-08-01 07:09:05 -03:00
|
|
|
// convert vector length into a speed
|
2017-12-05 21:41:28 -04:00
|
|
|
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
|
2017-08-01 07:09:05 -03:00
|
|
|
// convert vector direction to target yaw
|
|
|
|
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
|
2017-10-30 21:00:03 -03:00
|
|
|
|
2017-08-01 07:09:05 -03:00
|
|
|
// rotate target yaw if provided in body-frame
|
2017-05-03 11:21:58 -03:00
|
|
|
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
|
2017-08-01 07:09:05 -03:00
|
|
|
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2016-08-23 03:04:30 -03:00
|
|
|
}
|
|
|
|
|
2017-12-04 23:12:25 -04:00
|
|
|
// consume yaw heading
|
2017-08-01 07:09:05 -03:00
|
|
|
if (!yaw_ignore) {
|
|
|
|
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
|
|
|
|
// rotate target yaw if provided in body-frame
|
|
|
|
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
|
|
|
|
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
}
|
2017-12-04 23:12:25 -04:00
|
|
|
// consume yaw rate
|
|
|
|
float target_turn_rate_cds = 0.0f;
|
2017-08-01 07:09:05 -03:00
|
|
|
if (!yaw_rate_ignore) {
|
|
|
|
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
|
|
|
|
}
|
2017-12-04 23:12:25 -04:00
|
|
|
|
|
|
|
// handling case when both velocity and either yaw or yaw-rate are provided
|
2017-10-30 21:00:03 -03:00
|
|
|
// by default, we consider that the rover will drive forward
|
2017-12-04 23:12:25 -04:00
|
|
|
float speed_dir = 1.0f;
|
|
|
|
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
|
|
|
|
// Note: we are using the x-axis velocity to determine direction even though
|
|
|
|
// the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
|
2017-10-30 21:00:03 -03:00
|
|
|
if (is_negative(packet.vx)) {
|
2017-12-04 23:12:25 -04:00
|
|
|
speed_dir = -1.0f;
|
2017-10-30 21:00:03 -03:00
|
|
|
}
|
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
|
|
|
|
// set guided mode targets
|
2018-05-25 03:06:51 -03:00
|
|
|
if (!pos_ignore) {
|
2017-08-01 07:09:05 -03:00
|
|
|
// consume position target
|
2017-08-01 20:10:38 -03:00
|
|
|
rover.mode_guided.set_desired_location(target_loc);
|
2017-08-01 07:09:05 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
|
|
|
|
// consume velocity
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
|
2017-10-30 21:00:03 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
|
|
|
|
// consume velocity and turn rate
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
|
2017-10-30 21:00:03 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
|
2019-02-07 20:28:23 -04:00
|
|
|
// consume velocity and heading
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
|
2017-08-01 07:09:05 -03:00
|
|
|
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
|
|
|
|
// consume just target heading (probably only skid steering vehicles can do this)
|
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
|
|
|
|
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
|
2019-02-07 20:28:23 -04:00
|
|
|
// consume just turn rate (probably only skid steering vehicles can do this)
|
2017-08-01 07:09:05 -03:00
|
|
|
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2016-08-23 03:04:30 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
|
|
|
|
{
|
|
|
|
// decode packet
|
|
|
|
mavlink_set_position_target_global_int_t packet;
|
|
|
|
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
2017-07-18 23:19:08 -03:00
|
|
|
if (rover.control_mode != &rover.mode_guided) {
|
2016-08-23 03:04:30 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
// check for supported coordinate frames
|
2017-09-18 23:57:57 -03:00
|
|
|
if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
|
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
|
2016-08-23 03:04:30 -03:00
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
|
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
|
2017-09-18 23:57:57 -03:00
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
|
2016-08-23 03:04:30 -03:00
|
|
|
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
|
|
|
|
break;
|
|
|
|
}
|
2016-11-23 13:13:56 -04:00
|
|
|
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
|
2017-05-03 11:21:58 -03:00
|
|
|
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
|
|
|
|
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
|
2017-08-01 07:09:05 -03:00
|
|
|
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
|
|
|
|
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
|
2017-05-03 11:21:58 -03:00
|
|
|
|
2017-08-01 07:09:05 -03:00
|
|
|
// prepare target position
|
2017-05-03 11:21:58 -03:00
|
|
|
Location target_loc = rover.current_loc;
|
2016-08-23 03:04:30 -03:00
|
|
|
if (!pos_ignore) {
|
2017-05-03 11:21:58 -03:00
|
|
|
// sanity check location
|
|
|
|
if (!check_latlng(packet.lat_int, packet.lon_int)) {
|
|
|
|
// result = MAV_RESULT_FAILED;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
target_loc.lat = packet.lat_int;
|
|
|
|
target_loc.lng = packet.lon_int;
|
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
|
2017-05-03 11:21:58 -03:00
|
|
|
float target_speed = 0.0f;
|
2017-08-01 07:09:05 -03:00
|
|
|
float target_yaw_cd = 0.0f;
|
|
|
|
|
|
|
|
// consume velocity and convert to target speed and heading
|
2017-05-03 11:21:58 -03:00
|
|
|
if (!vel_ignore) {
|
2017-12-05 21:41:28 -04:00
|
|
|
const float speed_max = rover.control_mode->get_speed_default();
|
2017-08-01 07:09:05 -03:00
|
|
|
// convert vector length into a speed
|
2017-12-05 21:41:28 -04:00
|
|
|
target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
|
2017-08-01 07:09:05 -03:00
|
|
|
// convert vector direction to target yaw
|
|
|
|
target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
|
2017-10-30 21:00:03 -03:00
|
|
|
|
|
|
|
// rotate target yaw if provided in body-frame
|
|
|
|
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
|
|
|
|
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
|
|
|
|
}
|
2016-08-23 03:04:30 -03:00
|
|
|
}
|
|
|
|
|
2017-12-04 23:12:25 -04:00
|
|
|
// consume yaw heading
|
2017-08-01 07:09:05 -03:00
|
|
|
if (!yaw_ignore) {
|
|
|
|
target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
|
|
|
|
// rotate target yaw if provided in body-frame
|
|
|
|
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
|
|
|
|
target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
}
|
2017-12-04 23:12:25 -04:00
|
|
|
// consume yaw rate
|
|
|
|
float target_turn_rate_cds = 0.0f;
|
2017-08-01 07:09:05 -03:00
|
|
|
if (!yaw_rate_ignore) {
|
|
|
|
target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
|
|
|
|
}
|
2017-12-04 23:12:25 -04:00
|
|
|
|
|
|
|
// handling case when both velocity and either yaw or yaw-rate are provided
|
2017-10-30 21:00:03 -03:00
|
|
|
// by default, we consider that the rover will drive forward
|
2017-12-04 23:12:25 -04:00
|
|
|
float speed_dir = 1.0f;
|
|
|
|
if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
|
|
|
|
// Note: we are using the x-axis velocity to determine direction even though
|
|
|
|
// the frame is provided in MAV_FRAME_GLOBAL_xxx
|
2017-10-30 21:00:03 -03:00
|
|
|
if (is_negative(packet.vx)) {
|
2017-12-04 23:12:25 -04:00
|
|
|
speed_dir = -1.0f;
|
2017-10-30 21:00:03 -03:00
|
|
|
}
|
|
|
|
}
|
2017-08-01 07:09:05 -03:00
|
|
|
|
|
|
|
// set guided mode targets
|
2018-05-25 03:06:51 -03:00
|
|
|
if (!pos_ignore) {
|
2017-08-01 07:09:05 -03:00
|
|
|
// consume position target
|
2017-08-01 20:10:38 -03:00
|
|
|
rover.mode_guided.set_desired_location(target_loc);
|
2017-08-01 07:09:05 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
|
|
|
|
// consume velocity
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
|
2017-10-30 21:00:03 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
|
|
|
|
// consume velocity and turn rate
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
|
2017-10-30 21:00:03 -03:00
|
|
|
} else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
|
|
|
|
// consume velocity
|
2017-12-04 23:12:25 -04:00
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
|
2017-08-01 07:09:05 -03:00
|
|
|
} else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
|
|
|
|
// consume just target heading (probably only skid steering vehicles can do this)
|
|
|
|
rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
|
|
|
|
} else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
|
|
|
|
// consume just turn rate(probably only skid steering vehicles can do this)
|
|
|
|
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
|
2017-05-03 11:21:58 -03:00
|
|
|
}
|
2016-08-23 03:04:30 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2012-12-18 16:17:06 -04:00
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED
|
2016-11-23 13:13:56 -04:00
|
|
|
case MAVLINK_MSG_ID_HIL_STATE:
|
|
|
|
{
|
|
|
|
mavlink_hil_state_t packet;
|
|
|
|
mavlink_msg_hil_state_decode(msg, &packet);
|
|
|
|
|
2016-06-01 19:29:32 -03:00
|
|
|
// sanity check location
|
|
|
|
if (!check_latlng(packet.lat, packet.lon)) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
// set gps hil sensor
|
2014-03-30 22:01:54 -03:00
|
|
|
Location loc;
|
|
|
|
loc.lat = packet.lat;
|
|
|
|
loc.lng = packet.lon;
|
|
|
|
loc.alt = packet.alt/10;
|
|
|
|
Vector3f vel(packet.vx, packet.vy, packet.vz);
|
|
|
|
vel *= 0.01f;
|
2016-11-23 13:13:56 -04:00
|
|
|
|
2014-04-01 17:51:15 -03:00
|
|
|
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
|
2014-03-30 22:01:54 -03:00
|
|
|
packet.time_usec/1000,
|
2016-05-04 22:42:16 -03:00
|
|
|
loc, vel, 10, 0);
|
2016-11-23 13:13:56 -04:00
|
|
|
|
|
|
|
// rad/sec
|
2012-04-30 04:17:14 -03:00
|
|
|
Vector3f gyros;
|
2012-12-18 16:17:06 -04:00
|
|
|
gyros.x = packet.rollspeed;
|
|
|
|
gyros.y = packet.pitchspeed;
|
|
|
|
gyros.z = packet.yawspeed;
|
2012-12-18 15:30:42 -04:00
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
// m/s/s
|
|
|
|
Vector3f accels;
|
2013-01-10 14:42:24 -04:00
|
|
|
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
|
|
|
|
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
|
|
|
|
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
|
2016-11-23 13:13:56 -04:00
|
|
|
|
2014-02-22 17:18:15 -04:00
|
|
|
ins.set_gyro(0, gyros);
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2014-02-22 17:18:15 -04:00
|
|
|
ins.set_accel(0, accels);
|
2015-05-15 01:04:51 -03:00
|
|
|
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
|
|
|
|
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
|
2013-06-03 22:57:59 -03:00
|
|
|
break;
|
2016-11-23 13:13:56 -04:00
|
|
|
}
|
|
|
|
#endif // HIL_MODE
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2012-05-14 15:33:03 -03:00
|
|
|
case MAVLINK_MSG_ID_RADIO:
|
2013-08-24 04:58:37 -03:00
|
|
|
case MAVLINK_MSG_ID_RADIO_STATUS:
|
2012-05-14 15:33:03 -03:00
|
|
|
{
|
2019-01-18 00:23:42 -04:00
|
|
|
handle_radio_status(msg, rover.logger, rover.should_log(MASK_LOG_PM));
|
2012-05-14 15:33:03 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2016-05-04 12:46:01 -03:00
|
|
|
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
|
2017-07-13 08:36:44 -03:00
|
|
|
rover.rangefinder.handle_msg(msg);
|
2017-08-16 07:57:42 -03:00
|
|
|
rover.g2.proximity.handle_msg(msg);
|
2016-05-04 12:46:01 -03:00
|
|
|
break;
|
2018-09-06 07:18:24 -03:00
|
|
|
case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
|
|
|
|
rover.g2.proximity.handle_msg(msg);
|
|
|
|
break;
|
2016-05-04 12:46:01 -03:00
|
|
|
|
2016-11-06 20:06:37 -04:00
|
|
|
default:
|
|
|
|
handle_common_message(msg);
|
|
|
|
break;
|
2016-11-23 13:13:56 -04:00
|
|
|
} // end switch
|
|
|
|
} // end handle mavlink
|
2012-04-30 04:17:14 -03:00
|
|
|
|
|
|
|
/*
|
2012-12-18 07:44:12 -04:00
|
|
|
* a delay() callback that processes MAVLink packets. We set this as the
|
|
|
|
* callback in long running library initialisation routines to allow
|
|
|
|
* MAVLink to process packets while waiting for the initialisation to
|
|
|
|
* complete
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::mavlink_delay_cb()
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2015-05-12 04:00:25 -03:00
|
|
|
static uint32_t last_1hz, last_50hz, last_5s;
|
2018-05-08 02:57:58 -03:00
|
|
|
if (!gcs().chan(0).initialised) {
|
2016-11-23 13:13:56 -04:00
|
|
|
return;
|
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2017-06-14 22:24:33 -03:00
|
|
|
// don't allow potentially expensive logging calls:
|
2019-01-18 00:23:42 -04:00
|
|
|
logger.EnableWrites(false);
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2017-01-31 06:12:26 -04:00
|
|
|
const uint32_t tnow = millis();
|
2012-12-18 07:44:12 -04:00
|
|
|
if (tnow - last_1hz > 1000) {
|
|
|
|
last_1hz = tnow;
|
2017-07-07 22:16:56 -03:00
|
|
|
gcs().send_message(MSG_HEARTBEAT);
|
2018-12-17 22:07:15 -04:00
|
|
|
gcs().send_message(MSG_SYS_STATUS);
|
2012-12-18 07:44:12 -04:00
|
|
|
}
|
|
|
|
if (tnow - last_50hz > 20) {
|
|
|
|
last_50hz = tnow;
|
2018-11-21 22:47:22 -04:00
|
|
|
gcs().update_receive();
|
|
|
|
gcs().update_send();
|
2013-08-29 00:14:16 -03:00
|
|
|
notify.update();
|
2012-12-18 07:44:12 -04:00
|
|
|
}
|
|
|
|
if (tnow - last_5s > 5000) {
|
|
|
|
last_5s = tnow;
|
2017-07-08 22:40:59 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
|
2012-12-18 07:44:12 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
logger.EnableWrites(true);
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
|
2017-07-25 03:36:40 -03:00
|
|
|
AP_AdvancedFailsafe *GCS_MAVLINK_Rover::get_advanced_failsafe() const
|
|
|
|
{
|
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
|
|
|
return &rover.g2.afs;
|
|
|
|
#else
|
|
|
|
return nullptr;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2017-08-11 02:52:27 -03:00
|
|
|
bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
|
|
|
|
{
|
2018-06-06 03:41:17 -03:00
|
|
|
Mode *new_mode = rover.mode_from_mode_num((enum Mode::Number)mode);
|
2017-08-11 02:52:27 -03:00
|
|
|
if (new_mode == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return rover.set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
|
|
|
|
}
|