Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into pr-webots-heron-usv

This commit is contained in:
harunkurtdev 2024-11-13 23:01:49 +03:00 committed by GitHub
parent 00bc0ecc5d f08a2120a8
commit b07aef2305
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
158 changed files with 1996 additions and 514 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
AntennaTracker/GCS_Mavlink.cpp
View File

@ -276,10 +276,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_NAV_CONTROLLER_OUTPUT,
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
};
@ -315,7 +321,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_BATTERY_STATUS,
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
@ -643,3 +650,43 @@ void GCS_MAVLINK_Tracker::send_global_position_int()
0, // Z speed cm/s (+ve Down)
tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
}
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const
{
const Mode* modes[] {
&tracker.mode_manual,
&tracker.mode_stop,
&tracker.mode_scan,
&tracker.mode_guided,
&tracker.mode_servotest,
&tracker.mode_auto,
&tracker.mode_initialising,
};
const uint8_t mode_count = ARRAY_SIZE(modes);
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name();
const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
AntennaTracker/GCS_Mavlink.h
View File

@ -30,6 +30,10 @@ protected:
void send_nav_controller_output() const override;
void send_pid_tuning() override;
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;

2
AntennaTracker/Tracker.cpp
View File

@ -82,7 +82,7 @@ void Tracker::one_second_loop()
mavlink_system.sysid = g.sysid_this_mav;
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// updated armed/disarmed status LEDs
update_armed_disarmed();

8
AntennaTracker/mode.h
View File

@ -22,6 +22,7 @@ public:
// returns a unique number specific to this mode
virtual Mode::Number number() const = 0;
virtual const char* name() const = 0;
virtual bool requires_armed_servos() const = 0;
@ -41,6 +42,7 @@ protected:
class ModeAuto : public Mode {
public:
Mode::Number number() const override { return Mode::Number::AUTO; }
const char* name() const override { return "Auto"; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
@ -48,6 +50,7 @@ public:
class ModeGuided : public Mode {
public:
Mode::Number number() const override { return Mode::Number::GUIDED; }
const char* name() const override { return "Guided"; }
bool requires_armed_servos() const override { return true; }
void update() override;
@ -66,6 +69,7 @@ private:
class ModeInitialising : public Mode {
public:
Mode::Number number() const override { return Mode::Number::INITIALISING; }
const char* name() const override { return "Initialising"; }
bool requires_armed_servos() const override { return false; }
void update() override {};
};
@ -73,6 +77,7 @@ public:
class ModeManual : public Mode {
public:
Mode::Number number() const override { return Mode::Number::MANUAL; }
const char* name() const override { return "Manual"; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
@ -80,6 +85,7 @@ public:
class ModeScan : public Mode {
public:
Mode::Number number() const override { return Mode::Number::SCAN; }
const char* name() const override { return "Scan"; }
bool requires_armed_servos() const override { return true; }
void update() override;
};
@ -87,6 +93,7 @@ public:
class ModeServoTest : public Mode {
public:
Mode::Number number() const override { return Mode::Number::SERVOTEST; }
const char* name() const override { return "ServoTest"; }
bool requires_armed_servos() const override { return true; }
void update() override {};
@ -96,6 +103,7 @@ public:
class ModeStop : public Mode {
public:
Mode::Number number() const override { return Mode::Number::STOP; }
const char* name() const override { return "Stop"; }
bool requires_armed_servos() const override { return false; }
void update() override {};
};

2
AntennaTracker/servos.cpp
View File

@ -8,7 +8,7 @@
void Tracker::init_servos()
{
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw);
SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch);

2
ArduCopter/APM_Config.h
View File

@ -27,7 +27,7 @@
// features below are disabled by default on all boards
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes
//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link
//#define ADVANCED_FAILSAFE 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events
//#define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)

2
ArduCopter/AP_Arming.cpp
View File

@ -1,7 +1,7 @@
#include "Copter.h"
#pragma GCC diagnostic push
#if defined(__clang__)
#if defined(__clang_major__) && __clang_major__ >= 14
#pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical"
#endif

4
ArduCopter/Copter.cpp
View File

@ -232,7 +232,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100, 138),
#endif
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
SCHED_TASK(afs_fs_check, 10, 100, 141),
#endif
#if AP_TERRAIN_AVAILABLE
@ -727,7 +727,7 @@ void Copter::one_hz_loop()
}
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
#if HAL_LOGGING_ENABLED
// log terrain data

6
ArduCopter/Copter.h
View File

@ -137,7 +137,7 @@
#include <AP_OSD/AP_OSD.h>
#endif
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
# include "afs_copter.h"
#endif
#if TOY_MODE_ENABLED
@ -183,7 +183,7 @@ public:
friend class ParametersG2;
friend class AP_Avoidance_Copter;
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
friend class AP_AdvancedFailsafe_Copter;
#endif
friend class AP_Arming_Copter;
@ -804,7 +804,7 @@ private:
// failsafe.cpp
void failsafe_enable();
void failsafe_disable();
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
void afs_fs_check(void);
#endif

130
ArduCopter/GCS_Mavlink.cpp
View File

@ -505,10 +505,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
MSG_NAV_CONTROLLER_OUTPUT,
@ -583,7 +589,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
#endif
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
static const ap_message STREAM_ADSB_msgs[] = {
MSG_ADSB_VEHICLE,
@ -1519,7 +1526,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
}
MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) {
return MAV_RESULT_ACCEPTED;
}
@ -1657,3 +1664,120 @@ uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
{
const Mode* modes[] {
#if MODE_AUTO_ENABLED
&copter.mode_auto, // This auto is actually auto RTL!
&copter.mode_auto, // This one is really is auto!
#endif
#if MODE_ACRO_ENABLED
&copter.mode_acro,
#endif
&copter.mode_stabilize,
&copter.mode_althold,
#if MODE_CIRCLE_ENABLED
&copter.mode_circle,
#endif
#if MODE_LOITER_ENABLED
&copter.mode_loiter,
#endif
#if MODE_GUIDED_ENABLED
&copter.mode_guided,
#endif
&copter.mode_land,
#if MODE_RTL_ENABLED
&copter.mode_rtl,
#endif
#if MODE_DRIFT_ENABLED
&copter.mode_drift,
#endif
#if MODE_SPORT_ENABLED
&copter.mode_sport,
#endif
#if MODE_FLIP_ENABLED
&copter.mode_flip,
#endif
#if AUTOTUNE_ENABLED
&copter.mode_autotune,
#endif
#if MODE_POSHOLD_ENABLED
&copter.mode_poshold,
#endif
#if MODE_BRAKE_ENABLED
&copter.mode_brake,
#endif
#if MODE_THROW_ENABLED
&copter.mode_throw,
#endif
#if HAL_ADSB_ENABLED
&copter.mode_avoid_adsb,
#endif
#if MODE_GUIDED_NOGPS_ENABLED
&copter.mode_guided_nogps,
#endif
#if MODE_SMARTRTL_ENABLED
&copter.mode_smartrtl,
#endif
#if MODE_FLOWHOLD_ENABLED
(Mode*)copter.g2.mode_flowhold_ptr,
#endif
#if MODE_FOLLOW_ENABLED
&copter.mode_follow,
#endif
#if MODE_ZIGZAG_ENABLED
&copter.mode_zigzag,
#endif
#if MODE_SYSTEMID_ENABLED
(Mode *)copter.g2.mode_systemid_ptr,
#endif
#if MODE_AUTOROTATE_ENABLED
&copter.mode_autorotate,
#endif
#if MODE_TURTLE_ENABLED
&copter.mode_turtle,
#endif
};
const uint8_t mode_count = ARRAY_SIZE(modes);
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name();
uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number();
#if MODE_AUTO_ENABLED
// Auto RTL is odd
// Have to deal with is separately becase its number and name can change depending on if were in it or not
if (index_zero == 0) {
mode_number = (uint8_t)Mode::Number::AUTO_RTL;
name = "AUTO RTL";
} else if (index_zero == 1) {
mode_number = (uint8_t)Mode::Number::AUTO;
name = "AUTO";
}
#endif
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
ArduCopter/GCS_Mavlink.h
View File

@ -64,6 +64,10 @@ protected:
uint32_t log_radio_bit() const override { return MASK_LOG_PM; }
#endif
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
// sanity check velocity or acceleration vector components are numbers

23
ArduCopter/Parameters.cpp
View File

@ -771,7 +771,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1),
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// @Group: AFS_
// @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe),
@ -1240,8 +1240,11 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
/*
constructor for g2 object
*/
ParametersG2::ParametersG2(void)
: command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
ParametersG2::ParametersG2(void) :
unused_integer{17}
#if HAL_BUTTON_ENABLED
,button_ptr(&copter.button)
#endif
#if AP_TEMPCALIBRATION_ENABLED
, temp_calibration()
#endif
@ -1251,21 +1254,21 @@ ParametersG2::ParametersG2(void)
#if HAL_PROXIMITY_ENABLED
, proximity()
#endif
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
,afs()
#endif
#if MODE_SMARTRTL_ENABLED
,smart_rtl()
#endif
#if USER_PARAMS_ENABLED
,user_parameters()
#endif
#if MODE_FLOWHOLD_ENABLED
,mode_flowhold_ptr(&copter.mode_flowhold)
#endif
#if MODE_FOLLOW_ENABLED
,follow()
#endif
#if USER_PARAMS_ENABLED
,user_parameters()
#endif
#if AUTOTUNE_ENABLED
,autotune_ptr(&copter.mode_autotune.autotune)
#endif
@ -1275,13 +1278,9 @@ ParametersG2::ParametersG2(void)
#if MODE_AUTOROTATE_ENABLED
,arot()
#endif
#if HAL_BUTTON_ENABLED
,button_ptr(&copter.button)
#endif
#if MODE_ZIGZAG_ENABLED
,mode_zigzag_ptr(&copter.mode_zigzag)
#endif
#if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED
,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f)
#endif
@ -1290,6 +1289,8 @@ ParametersG2::ParametersG2(void)
,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f)
#endif
,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
#if WEATHERVANE_ENABLED
,weathervane()
#endif

9
ArduCopter/Parameters.h
View File

@ -499,6 +499,11 @@ public:
// altitude at which nav control can start in takeoff
AP_Float wp_navalt_min;
// unused_integer simply exists so that the constructor for
// ParametersG2 can be created with a relatively easy syntax in
// the face of many #ifs:
uint8_t unused_integer;
// button checking
#if HAL_BUTTON_ENABLED
AP_Button *button_ptr;
@ -530,8 +535,8 @@ public:
// whether to enforce acceptance of packets only from sysid_my_gcs
AP_Int8 sysid_enforce;
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// advanced failsafe library
AP_AdvancedFailsafe_Copter afs;
#endif

4
ArduCopter/afs_copter.cpp
View File

@ -4,7 +4,7 @@
#include "Copter.h"
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
/*
setup radio_out values for all channels to termination values
@ -63,4 +63,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
{
copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE);
}
#endif // ADVANCED_FAILSAFE
#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED

6
ArduCopter/afs_copter.h
View File

@ -18,7 +18,9 @@
advanced failsafe support for copter
*/
#if ADVANCED_FAILSAFE
#include "config.h"
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
/*
@ -44,5 +46,5 @@ protected:
void set_mode_auto(void) override;
};
#endif // ADVANCED_FAILSAFE
#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED

5
ArduCopter/compassmot.cpp
View File

@ -146,9 +146,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
read_radio();
// pass through throttle to motors
SRV_Channels::cork();
auto &srv = AP::srv();
srv.cork();
motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
SRV_Channels::push();
srv.push();
// read some compass values
compass.read();

4
ArduCopter/config.h
View File

@ -574,8 +574,8 @@
// Developer Items
//
#ifndef ADVANCED_FAILSAFE
# define ADVANCED_FAILSAFE 0
#ifndef AP_COPTER_ADVANCED_FAILSAFE_ENABLED
# define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0
#endif
#ifndef CH_MODE_DEFAULT

18
ArduCopter/esc_calibration.cpp
View File

@ -95,9 +95,10 @@ void Copter::esc_calibration_passthrough()
hal.scheduler->delay(3);
// pass through to motors
SRV_Channels::cork();
auto &srv = AP::srv();
srv.cork();
motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f);
SRV_Channels::push();
srv.push();
}
#endif // FRAME_CONFIG != HELI_FRAME
}
@ -112,25 +113,26 @@ void Copter::esc_calibration_auto()
esc_calibration_setup();
// raise throttle to maximum
SRV_Channels::cork();
auto &srv = AP::srv();
srv.cork();
motors->set_throttle_passthrough_for_esc_calibration(1.0f);
SRV_Channels::push();
srv.push();
// delay for 5 seconds while outputting pulses
uint32_t tstart = millis();
while (millis() - tstart < 5000) {
SRV_Channels::cork();
srv.cork();
motors->set_throttle_passthrough_for_esc_calibration(1.0f);
SRV_Channels::push();
srv.push();
esc_calibration_notify();
hal.scheduler->delay(3);
}
// block until we restart
while(1) {
SRV_Channels::cork();
srv.cork();
motors->set_throttle_passthrough_for_esc_calibration(0.0f);
SRV_Channels::push();
srv.push();
esc_calibration_notify();
hal.scheduler->delay(3);
}

2
ArduCopter/events.cpp
View File

@ -492,7 +492,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
set_mode_SmartRTL_or_land_with_pause(reason);
break;
case FailsafeAction::TERMINATE: {
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
g2.afs.gcs_terminate(true, "Failsafe");
#else
arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);

2
ArduCopter/failsafe.cpp
View File

@ -72,7 +72,7 @@ void Copter::failsafe_check()
}
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
/*
check for AFS failsafe check
*/

12
ArduCopter/mode.h
View File

@ -4,7 +4,7 @@
#include <AP_Math/chirp.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
#include "afs_copter.h"
#endif
@ -134,7 +134,7 @@ public:
virtual bool allows_flip() const { return false; }
virtual bool crash_check_enabled() const { return true; }
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// Return the type of this mode for use by advanced failsafe
virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; }
#endif
@ -517,7 +517,7 @@ public:
bool allows_inverted() const override { return true; };
#endif
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
@ -1068,7 +1068,7 @@ public:
bool requires_terrain_failsafe() const override { return true; }
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
@ -1243,7 +1243,7 @@ public:
bool is_landing() const override { return true; };
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
@ -1425,7 +1425,7 @@ public:
bool requires_terrain_failsafe() const override { return true; }
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif

8
ArduCopter/mode_poshold.cpp
View File

@ -535,11 +535,7 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel
}
// calculate velocity-only based lean angle
if (velocity >= 0) {
lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f));
} else {
lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f));
}
lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (fabsf(velocity) + 60.0f));
// do not let lean_angle be too far from brake_angle
brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate);
@ -592,7 +588,7 @@ void ModePosHold::update_wind_comp_estimate()
}
// limit acceleration
const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f;
const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * (GRAVITY_MSS*100);
const float wind_comp_ef_len = wind_comp_ef.length();
if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) {
wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len;

8
ArduCopter/motors.cpp
View File

@ -135,7 +135,7 @@ void Copter::auto_disarm_check()
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
@ -156,8 +156,10 @@ void Copter::motors_output()
// output any servo channels
SRV_Channels::calc_pwm();
auto &srv = AP::srv();
// cork now, so that all channel outputs happen at once
SRV_Channels::cork();
srv.cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
@ -181,7 +183,7 @@ void Copter::motors_output()
}
// push all channels
SRV_Channels::push();
srv.push();
}
// check for pilot stick input to trigger lost vehicle alarm

2
ArduCopter/radio.cpp
View File

@ -45,7 +45,7 @@ void Copter::init_rc_out()
motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
// enable aux servos to cope with multiple output channels per motor
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// update rate must be set after motors->init() to allow for motor mapping
motors->set_update_rate(g.rc_speed);

4
ArduPlane/ArduPlane.cpp
View File

@ -329,7 +329,7 @@ void Plane::one_second_loop()
set_control_channels();
#if HAL_WITH_IO_MCU
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
#endif
#if HAL_ADSB_ENABLED
@ -346,7 +346,7 @@ void Plane::one_second_loop()
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);

126
ArduPlane/GCS_Mavlink.cpp
View File

@ -630,10 +630,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_CURRENT_WAYPOINT,
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
MSG_NAV_CONTROLLER_OUTPUT,
@ -710,7 +716,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_VIBRATION,
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
static const ap_message STREAM_ADSB_msgs[] = {
MSG_ADSB_VEHICLE,
@ -863,6 +870,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == &plane.mode_guided)) {
plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
#endif
// add home alt if needed
if (requested_position.relative_alt) {
@ -973,14 +983,20 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl
float new_target_heading = radians(wrap_180(packet.param2));
// course over ground
if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int
switch(HEADING_TYPE(packet.param1)) {
case HEADING_TYPE_COURSE_OVER_GROUND:
// course over ground
plane.guided_state.target_heading_type = GUIDED_HEADING_COG;
plane.prev_WP_loc = plane.current_loc;
// normal vehicle heading
} else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int
break;
case HEADING_TYPE_HEADING:
// normal vehicle heading
plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING;
} else {
break;
case HEADING_TYPE_DEFAULT:
plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
return MAV_RESULT_ACCEPTED;
default:
// MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters).
return MAV_RESULT_DENIED;
}
@ -1530,3 +1546,99 @@ MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const
return MAV_LANDED_STATE_ON_GROUND;
}
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const
{
// Fixed wing modes
const Mode* fw_modes[] {
&plane.mode_manual,
&plane.mode_circle,
&plane.mode_stabilize,
&plane.mode_training,
&plane.mode_acro,
&plane.mode_fbwa,
&plane.mode_fbwb,
&plane.mode_cruise,
&plane.mode_autotune,
&plane.mode_auto,
&plane.mode_rtl,
&plane.mode_loiter,
#if HAL_ADSB_ENABLED
&plane.mode_avoidADSB,
#endif
&plane.mode_guided,
&plane.mode_initializing,
&plane.mode_takeoff,
#if HAL_SOARING_ENABLED
&plane.mode_thermal,
#endif
};
const uint8_t fw_mode_count = ARRAY_SIZE(fw_modes);
// Fixedwing modes are always present
uint8_t mode_count = fw_mode_count;
#if HAL_QUADPLANE_ENABLED
// Quadplane modes
const Mode* q_modes[] {
&plane.mode_qstabilize,
&plane.mode_qhover,
&plane.mode_qloiter,
&plane.mode_qland,
&plane.mode_qrtl,
&plane.mode_qacro,
&plane.mode_loiter_qland,
#if QAUTOTUNE_ENABLED
&plane.mode_qautotune,
#endif
};
// Quadplane modes must be enabled
if (plane.quadplane.available()) {
mode_count += ARRAY_SIZE(q_modes);
}
#endif // HAL_QUADPLANE_ENABLED
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name;
uint8_t mode_number;
if (index_zero < fw_mode_count) {
// A fixedwing mode
name = fw_modes[index_zero]->name();
mode_number = (uint8_t)fw_modes[index_zero]->mode_number();
} else {
#if HAL_QUADPLANE_ENABLED
// A Quadplane mode
const uint8_t q_index = index_zero - fw_mode_count;
name = q_modes[q_index]->name();
mode_number = (uint8_t)q_modes[q_index]->mode_number();
#else
// Should not endup here
return mode_count;
#endif
}
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
ArduPlane/GCS_Mavlink.h
View File

@ -50,6 +50,10 @@ protected:
void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override;
void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
void send_pid_info(const struct AP_PIDInfo *pid_info, const uint8_t axis, const float achieved);

9
ArduPlane/Parameters.cpp
View File

@ -158,6 +158,15 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Standard
ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 0),
// @Param: TKOFF_THR_IDLE
// @DisplayName: Takeoff idle throttle
// @Description: The idle throttle to hold after arming and before a takeoff. Applicable in TAKEOFF and AUTO modes.
// @Units: %
// @Range: 0 100
// @Increment: 1
// @User: Standard
ASCALAR(takeoff_throttle_idle, "TKOFF_THR_IDLE", 0),
// @Param: TKOFF_OPTIONS
// @DisplayName: Takeoff options
// @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes.

7
ArduPlane/Parameters.h
View File

@ -359,6 +359,7 @@ public:
k_param_autotune_options,
k_param_takeoff_throttle_min,
k_param_takeoff_options,
k_param_takeoff_throttle_idle,
k_param_pullup = 270,
};
@ -483,6 +484,9 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
// just to make compilation easier when all things are compiled out...
uint8_t unused_integer;
// button reporting library
#if HAL_BUTTON_ENABLED
AP_Button *button_ptr;
@ -579,9 +583,6 @@ public:
AP_Int8 axis_bitmask; // axes to be autotuned
// just to make compilation easier when all things are compiled out...
uint8_t unused_integer;
#if AP_RANGEFINDER_ENABLED
// orientation of rangefinder to use for landing
AP_Int8 rangefinder_land_orient;

12
ArduPlane/defines.h
View File

@ -64,18 +64,6 @@ enum class RtlAutoland {
};
enum ChannelMixing {
MIXING_DISABLED = 0,
MIXING_UPUP = 1,
MIXING_UPDN = 2,
MIXING_DNUP = 3,
MIXING_DNDN = 4,
MIXING_UPUP_SWP = 5,
MIXING_UPDN_SWP = 6,
MIXING_DNUP_SWP = 7,
MIXING_DNDN_SWP = 8,
};
// PID broadcast bitmask
enum tuning_pid_bits {
TUNING_BITS_ROLL = (1 << 0),

7
ArduPlane/mode.cpp
View File

@ -1,14 +1,15 @@
#include "Plane.h"
Mode::Mode() :
ahrs(plane.ahrs)
unused_integer{17},
#if HAL_QUADPLANE_ENABLED
, quadplane(plane.quadplane),
pos_control(plane.quadplane.pos_control),
attitude_control(plane.quadplane.attitude_control),
loiter_nav(plane.quadplane.loiter_nav),
poscontrol(plane.quadplane.poscontrol)
quadplane(plane.quadplane),
poscontrol(plane.quadplane.poscontrol),
#endif
ahrs(plane.ahrs)
{
}

3
ArduPlane/mode.h
View File

@ -159,6 +159,9 @@ protected:
// Output pilot throttle, this is used in stabilized modes without auto throttle control
void output_pilot_throttle();
// makes the initialiser list in the constructor manageable
uint8_t unused_integer;
#if HAL_QUADPLANE_ENABLED
// References for convenience, used by QModes
AC_PosControl*& pos_control;

1
ArduPlane/mode_qland.cpp
View File

@ -9,7 +9,6 @@ bool ModeQLand::_enter()
quadplane.throttle_wait = false;
quadplane.setup_target_position();
poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND);
poscontrol.pilot_correction_done = false;
quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing);
quadplane.landing_detect.lower_limit_start_ms = 0;
quadplane.landing_detect.land_start_ms = 0;

5
ArduPlane/quadplane.cpp
View File

@ -4596,6 +4596,11 @@ void QuadPlane::mode_enter(void)
poscontrol.last_velocity_match_ms = 0;
poscontrol.set_state(QuadPlane::QPOS_NONE);
// Clear any pilot corrections
poscontrol.pilot_correction_done = false;
poscontrol.pilot_correction_active = false;
poscontrol.target_vel_cms.zero();
// clear guided takeoff wait on any mode change, but remember the
// state for special behaviour
guided_wait_takeoff_on_mode_enter = guided_wait_takeoff;

2
ArduPlane/radio.cpp
View File

@ -107,7 +107,7 @@ void Plane::init_rc_out_main()
*/
void Plane::init_rc_out_aux()
{
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
servos_output();

14
ArduPlane/servos.cpp
View File

@ -619,6 +619,11 @@ void Plane::set_throttle(void)
// throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines:
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get());
} else if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF)
&& (aparm.takeoff_throttle_idle.get() > 0)
) {
// we want to spin at idle throttle before the takeoff conditions are met
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.takeoff_throttle_idle.get());
} else {
// default
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
@ -853,8 +858,8 @@ void Plane::set_servos(void)
// start with output corked. the cork is released when we run
// servos_output(), which is run from all code paths in this
// function
SRV_Channels::cork();
AP::srv().cork();
// this is to allow the failsafe module to deliberately crash
// the plane. Only used in extreme circumstances to meet the
// OBC rules
@ -1012,7 +1017,8 @@ void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void)
*/
void Plane::servos_output(void)
{
SRV_Channels::cork();
auto &srv = AP::srv();
srv.cork();
// support twin-engine aircraft
servos_twin_engine_mix();
@ -1050,7 +1056,7 @@ void Plane::servos_output(void)
SRV_Channels::output_ch_all();
SRV_Channels::push();
srv.push();
if (g2.servo_channels.auto_trim_enabled()) {
servos_auto_trim();

2
ArduSub/ArduSub.cpp
View File

@ -290,7 +290,7 @@ void Sub::one_hz_loop()
}
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
#if HAL_LOGGING_ENABLED
// log terrain data

55
ArduSub/GCS_Mavlink.cpp
View File

@ -369,10 +369,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_CURRENT_WAYPOINT,
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
MSG_NAV_CONTROLLER_OUTPUT,
@ -437,7 +443,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
#endif
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
@ -962,3 +969,47 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Sub::send_available_mode(uint8_t index) const
{
const Mode* modes[] {
&sub.mode_manual,
&sub.mode_stabilize,
&sub.mode_acro,
&sub.mode_althold,
&sub.mode_surftrak,
&sub.mode_poshold,
&sub.mode_auto,
&sub.mode_guided,
&sub.mode_circle,
&sub.mode_surface,
&sub.mode_motordetect,
};
const uint8_t mode_count = ARRAY_SIZE(modes);
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name();
const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
ArduSub/GCS_Mavlink.h
View File

@ -37,6 +37,10 @@ protected:
uint64_t capabilities() const override;
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
void handle_message(const mavlink_message_t &msg) override;

15
ArduSub/mode.h
View File

@ -72,6 +72,9 @@ public:
virtual const char *name() const = 0;
virtual const char *name4() const = 0;
// returns a unique number specific to this mode
virtual Mode::Number number() const = 0;
// functions for reporting to GCS
virtual bool get_wp(Location &loc) { return false; }
virtual int32_t wp_bearing() const { return 0; }
@ -202,6 +205,7 @@ protected:
const char *name() const override { return "MANUAL"; }
const char *name4() const override { return "MANU"; }
Mode::Number number() const override { return Mode::Number::MANUAL; }
};
@ -224,6 +228,7 @@ protected:
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
Mode::Number number() const override { return Mode::Number::ACRO; }
};
@ -246,6 +251,7 @@ protected:
const char *name() const override { return "STABILIZE"; }
const char *name4() const override { return "STAB"; }
Mode::Number number() const override { return Mode::Number::STABILIZE; }
};
@ -272,6 +278,7 @@ protected:
const char *name() const override { return "ALT_HOLD"; }
const char *name4() const override { return "ALTH"; }
Mode::Number number() const override { return Mode::Number::ALT_HOLD; }
};
@ -293,6 +300,7 @@ protected:
const char *name() const override { return "SURFTRAK"; }
const char *name4() const override { return "STRK"; }
Mode::Number number() const override { return Mode::Number::SURFTRAK; }
private:
@ -342,6 +350,8 @@ protected:
const char *name() const override { return "GUIDED"; }
const char *name4() const override { return "GUID"; }
Mode::Number number() const override { return Mode::Number::GUIDED; }
autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const;
private:
@ -387,6 +397,7 @@ protected:
const char *name() const override { return "AUTO"; }
const char *name4() const override { return "AUTO"; }
Mode::Number number() const override { return Mode::Number::AUTO; }
private:
void auto_wp_run();
@ -417,6 +428,7 @@ protected:
const char *name() const override { return "POSHOLD"; }
const char *name4() const override { return "POSH"; }
Mode::Number number() const override { return Mode::Number::POSHOLD; }
};
@ -439,6 +451,7 @@ protected:
const char *name() const override { return "CIRCLE"; }
const char *name4() const override { return "CIRC"; }
Mode::Number number() const override { return Mode::Number::CIRCLE; }
};
class ModeSurface : public Mode
@ -460,6 +473,7 @@ protected:
const char *name() const override { return "SURFACE"; }
const char *name4() const override { return "SURF"; }
Mode::Number number() const override { return Mode::Number::CIRCLE; }
};
@ -482,4 +496,5 @@ protected:
const char *name() const override { return "MOTORDETECT"; }
const char *name4() const override { return "DETE"; }
Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; }
};

5
ArduSub/motors.cpp
View File

@ -18,11 +18,12 @@ void Sub::motors_output()
verify_motor_test();
} else {
motors.set_interlock(true);
SRV_Channels::cork();
auto &srv = AP::srv();
srv.cork();
SRV_Channels::calc_pwm();
SRV_Channels::output_ch_all();
motors.output();
SRV_Channels::push();
srv.push();
}
}

2
Blimp/Blimp.cpp
View File

@ -210,7 +210,7 @@ void Blimp::one_hz_loop()
#endif
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
AP_Notify::flags.flying = !ap.land_complete;

49
Blimp/GCS_Mavlink.cpp
View File

@ -327,10 +327,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
MSG_NAV_CONTROLLER_OUTPUT,
@ -395,7 +401,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
#endif
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
static const ap_message STREAM_ADSB_msgs[] = {
MSG_ADSB_VEHICLE,
@ -608,3 +615,41 @@ uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const
return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
}
#endif // HAL_HIGH_LATENCY2_ENABLED
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Blimp::send_available_mode(uint8_t index) const
{
const Mode* modes[] {
&blimp.mode_land,
&blimp.mode_manual,
&blimp.mode_velocity,
&blimp.mode_loiter,
&blimp.mode_rtl,
};
const uint8_t mode_count = ARRAY_SIZE(modes);
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name();
const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
Blimp/GCS_Mavlink.h
View File

@ -48,6 +48,10 @@ protected:
uint32_t log_radio_bit() const override { return MASK_LOG_PM; }
#endif
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
void handle_message(const mavlink_message_t &msg) override;

14
Blimp/mode.h
View File

@ -52,6 +52,9 @@ public:
virtual const char *name() const = 0;
virtual const char *name4() const = 0;
// returns a unique number specific to this mode
virtual Mode::Number number() const = 0;
virtual bool is_landing() const
{
return false;
@ -159,6 +162,8 @@ protected:
return "MANU";
}
Mode::Number number() const override { return Mode::Number::MANUAL; }
private:
};
@ -201,6 +206,8 @@ protected:
return "VELY";
}
Mode::Number number() const override { return Mode::Number::VELOCITY; }
private:
};
@ -244,6 +251,8 @@ protected:
return "LOIT";
}
Mode::Number number() const override { return Mode::Number::LOITER; }
private:
Vector3f target_pos;
float target_yaw;
@ -286,6 +295,8 @@ protected:
return "LAND";
}
Mode::Number number() const override { return Mode::Number::LAND; }
private:
};
@ -328,4 +339,7 @@ protected:
{
return "RTL";
}
Mode::Number number() const override { return Mode::Number::RTL; }
};

8
Blimp/motors.cpp
View File

@ -78,8 +78,10 @@ void Blimp::motors_output()
// output any servo channels
SRV_Channels::calc_pwm();
auto &srv = AP::srv();
// cork now, so that all channel outputs happen at once
SRV_Channels::cork();
srv.cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
@ -88,5 +90,5 @@ void Blimp::motors_output()
motors->output();
// push all channels
SRV_Channels::push();
}
srv.push();
}

2
Blimp/radio.cpp
View File

@ -38,7 +38,7 @@ void Blimp::init_rc_in()
void Blimp::init_rc_out()
{
// enable aux servos to cope with multiple output channels per motor
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// refresh auxiliary channel to function map
SRV_Channels::update_aux_servo_function();

62
Rover/GCS_Mavlink.cpp
View File

@ -541,10 +541,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
#endif
MSG_MEMINFO,
MSG_CURRENT_WAYPOINT,
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#if GPS_MAX_RECEIVERS > 1
#endif
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
MSG_GPS2_RAW,
#endif
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
MSG_GPS2_RTK,
#endif
MSG_NAV_CONTROLLER_OUTPUT,
@ -613,7 +619,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
#endif
};
static const ap_message STREAM_PARAMS_msgs[] = {
MSG_NEXT_PARAM
MSG_NEXT_PARAM,
MSG_AVAILABLE_MODES
};
static const ap_message STREAM_ADSB_msgs[] = {
MSG_ADSB_VEHICLE,
@ -1148,3 +1155,54 @@ uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
return 0;
}
#endif // HAL_HIGH_LATENCY2_ENABLED
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const
{
const Mode* modes[] {
&rover.mode_manual,
&rover.mode_acro,
&rover.mode_steering,
&rover.mode_hold,
&rover.mode_loiter,
#if MODE_FOLLOW_ENABLED
&rover.mode_follow,
#endif
&rover.mode_simple,
&rover.g2.mode_circle,
&rover.mode_auto,
&rover.mode_rtl,
&rover.mode_smartrtl,
&rover.mode_guided,
&rover.mode_initializing,
#if MODE_DOCK_ENABLED
(Mode *)rover.g2.mode_dock_ptr,
#endif
};
const uint8_t mode_count = ARRAY_SIZE(modes);
// Convert to zero indexed
const uint8_t index_zero = index - 1;
if (index_zero >= mode_count) {
// Mode does not exist!?
return mode_count;
}
// Ask the mode for its name and number
const char* name = modes[index_zero]->name4();
const uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number();
mavlink_msg_available_modes_send(
chan,
mode_count,
index,
MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
mode_number,
0, // MAV_MODE_PROPERTY bitmask
name
);
return mode_count;
}

4
Rover/GCS_Mavlink.h
View File

@ -40,6 +40,10 @@ protected:
uint32_t log_radio_bit() const override { return MASK_LOG_PM; }
#endif
// Send the mode with the given index (not mode number!) return the total number of modes
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
private:
void handle_message(const mavlink_message_t &msg) override;

2
Rover/Rover.cpp
View File

@ -435,7 +435,7 @@ void Rover::one_second_loop(void)
set_control_channels();
// cope with changes to aux functions
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);

2
Rover/system.cpp
View File

@ -70,7 +70,7 @@ void Rover::init_ardupilot()
init_rc_in(); // sets up rc channels deadzone
g2.motors.init(get_frame_type()); // init motors including setting servo out channels ranges
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
// init wheel encoders
g2.wheel_encoder.init();

4
Tools/AP_Periph/GCS_MAVLink.cpp
View File

@ -32,8 +32,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
MSG_MCU_STATUS,
#endif
MSG_MEMINFO,
#if AP_GPS_ENABLED
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
MSG_GPS_RAW,
#endif
#if AP_GPS_GPS_RTK_SENDING_ENABLED
MSG_GPS_RTK,
#endif
};

5
Tools/AP_Periph/esc_apd_telem.cpp
View File

@ -17,8 +17,9 @@ extern const AP_HAL::HAL& hal;
#define TELEM_LEN 0x16
ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) :
pole_count(num_poles),
uart(_uart) {
uart(_uart),
pole_count(num_poles)
{
uart->begin(115200);
}

9
Tools/AP_Periph/rc_out.cpp
View File

@ -71,7 +71,7 @@ void AP_Periph_FW::rcout_init()
hal.rcout->set_freq(esc_mask, g.esc_rate.get());
// setup ESCs with the desired PWM type, allowing for DShot
SRV_Channels::init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get());
AP::srv().init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get());
// run DShot at 1kHz
hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), 400);
@ -83,7 +83,7 @@ void AP_Periph_FW::rcout_init()
void AP_Periph_FW::rcout_init_1Hz()
{
// this runs at 1Hz to allow for run-time param changes
SRV_Channels::enable_aux_servos();
AP::srv().enable_aux_servos();
for (uint8_t i=0; i<SERVO_OUT_MOTOR_MAX; i++) {
servo_channels.set_esc_scaling_for(SRV_Channels::get_motor_function(i));
@ -156,10 +156,11 @@ void AP_Periph_FW::rcout_update()
}
rcout_has_new_data_to_update = false;
auto &srv = AP::srv();
SRV_Channels::calc_pwm();
SRV_Channels::cork();
srv.cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
srv.push();
#if HAL_WITH_ESC_TELEM
if (now_ms - last_esc_telem_update_ms >= esc_telem_update_period_ms) {
last_esc_telem_update_ms = now_ms;

2
Tools/ardupilotwaf/boards.py
View File

@ -361,10 +361,10 @@ class Board:
'-Wpointer-arith',
'-Wno-unused-parameter',
'-Wno-missing-field-initializers',
'-Wno-reorder',
'-Wno-redundant-decls',
'-Wno-unknown-pragmas',
'-Wno-expansion-to-defined',
'-Werror=reorder',
'-Werror=cast-align',
'-Werror=attributes',
'-Werror=format-security',

29
Tools/autotest/arduplane.py
View File

@ -4831,6 +4831,34 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.fly_home_land_and_disarm()
def TakeoffIdleThrottle(self):
'''Apply idle throttle before takeoff.'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"TKOFF_THR_IDLE": 20.0,
"TKOFF_THR_MINSPD": 3.0,
})
self.change_mode("TAKEOFF")
self.wait_ready_to_arm()
self.arm_vehicle()
# Ensure that the throttle rises to idle throttle.
expected_idle_throttle = 1000+10*self.get_parameter("TKOFF_THR_IDLE")
self.assert_servo_channel_range(3, expected_idle_throttle-10, expected_idle_throttle+10)
# Launch the catapult
self.set_servo(6, 1000)
self.delay_sim_time(5)
self.change_mode('RTL')
self.fly_home_land_and_disarm()
def DCMFallback(self):
'''Really annoy the EKF and force fallback'''
self.reboot_sitl()
@ -6389,6 +6417,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.TakeoffTakeoff3,
self.TakeoffTakeoff4,
self.TakeoffGround,
self.TakeoffIdleThrottle,
self.ForcedDCM,
self.DCMFallback,
self.MAVFTP,

43
Tools/autotest/helicopter.py
View File

@ -708,6 +708,48 @@ class AutoTestHelicopter(AutoTestCopter):
self.change_mode('LOITER')
self.fly_mission_points(self.scurve_nasty_up_mission())
def MountFailsafeAction(self):
"""Fly Mount Failsafe action"""
self.context_push()
self.progress("Setting up servo mount")
roll_servo = 12
pitch_servo = 11
yaw_servo = 10
open_servo = 9
roll_limit = 50
self.set_parameters({
"MNT1_TYPE": 1,
"SERVO%u_MIN" % roll_servo: 1000,
"SERVO%u_MAX" % roll_servo: 2000,
"SERVO%u_FUNCTION" % yaw_servo: 6, # yaw
"SERVO%u_FUNCTION" % pitch_servo: 7, # roll
"SERVO%u_FUNCTION" % roll_servo: 8, # pitch
"SERVO%u_FUNCTION" % open_servo: 9, # mount open
"MNT1_OPTIONS": 2, # retract
"MNT1_DEFLT_MODE": 3, # RC targettting
"MNT1_ROLL_MIN": -roll_limit,
"MNT1_ROLL_MAX": roll_limit,
})
self.reboot_sitl()
retract_roll = 25.0
self.set_parameter("MNT1_NEUTRAL_X", retract_roll)
self.progress("Killing RC")
self.set_parameter("SIM_RC_FAIL", 2)
self.delay_sim_time(10)
want_servo_channel_value = int(1500 + 500*retract_roll/roll_limit)
self.wait_servo_channel_value(roll_servo, want_servo_channel_value, epsilon=1)
self.progress("Resurrecting RC")
self.set_parameter("SIM_RC_FAIL", 0)
self.wait_servo_channel_value(roll_servo, 1500)
self.context_pop()
self.reboot_sitl()
def set_rc_default(self):
super(AutoTestHelicopter, self).set_rc_default()
self.progress("Lowering rotor speed")
@ -1122,6 +1164,7 @@ class AutoTestHelicopter(AutoTestCopter):
self.NastyMission,
self.PIDNotches,
self.AutoTune,
self.MountFailsafeAction,
])
return ret

28
Tools/autotest/logger_metadata/enum_parse.py
View File

@ -92,6 +92,11 @@ class EnumDocco(object):
if m is not None:
return (m.group(1), 1 << int(m.group(2)), m.group(3))
# Match: "#define FRED 1 // optional comment"
m = re.match(r"#define\s*([A-Z0-9_a-z]+)\s+(-?\d+) *(// *(.*) *)?$", line)
if m is not None:
return (m.group(1), m.group(2), m.group(4))
if m is None:
raise ValueError("Failed to match (%s)" % line)
@ -116,7 +121,13 @@ class EnumDocco(object):
break
line = line.rstrip()
# print("state=%s line: %s" % (state, line))
if re.match(r"\s*//.*", line):
# Skip single-line comments - unless they contain LoggerEnum tags
if re.match(r"\s*//.*", line) and "LoggerEnum" not in line:
continue
# Skip multi-line comments
if re.match(r"\s*/\*.*", line):
while "*/" not in line:
line = f.readline()
continue
if state == "outside":
if re.match("class .*;", line) is not None:
@ -154,6 +165,19 @@ class EnumDocco(object):
last_value = None
state = state_inside
skip_enumeration = False
continue
# // @LoggerEnum: NAME - can be used around for #define sets
m = re.match(r".*@LoggerEnum: *([\w]+)", line)
if m is not None:
enum_name = m.group(1)
debug("%s: %s" % (source_file, enum_name))
entries = []
last_value = None
state = state_inside
skip_enumeration = False
continue
continue
if state == "inside":
if re.match(r"\s*$", line):
@ -164,7 +188,7 @@ class EnumDocco(object):
continue
if re.match(r"#else", line):
continue
if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line):
if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line) or "@LoggerEnumEnd" in line:
# potential end of enumeration
if not skip_enumeration:
if enum_name is None:

7
Tools/autotest/sim_vehicle.py
View File

@ -964,6 +964,8 @@ def start_mavproxy(opts, stuff):
cmd.extend(['--aircraft', opts.aircraft])
if opts.moddebug:
cmd.append('--moddebug=%u' % opts.moddebug)
if opts.mavcesium:
cmd.extend(["--load-module", "cesium"])
if opts.fresh_params:
# these were built earlier:
@ -1373,6 +1375,11 @@ group.add_option("", "--map",
default=False,
action='store_true',
help="load map module on startup")
group.add_option("", "--mavcesium",
default=False,
action='store_true',
help="load MAVCesium module on startup")
group.add_option("", "--console",
default=False,
action='store_true',

2
Tools/autotest/test_build_options.py
View File

@ -258,7 +258,6 @@ class TestBuildOptions(object):
'AP_COMPASS_MAG3110_ENABLED', # must be in hwdef, not probed
'AP_COMPASS_MMC5XX3_ENABLED', # must be in hwdef, not probed
'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', # completely elided
'AP_MAVLINK_MSG_HIL_GPS_ENABLED', # no symbol available
'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', # no symbol available
'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', # no symbol available
'HAL_MSP_SENSORS_ENABLED', # no symbol available
@ -287,6 +286,7 @@ class TestBuildOptions(object):
feature_define_whitelist.add('AP_WINCH_DAIWA_ENABLED')
feature_define_whitelist.add('AP_WINCH_PWM_ENABLED')
feature_define_whitelist.add(r'AP_MOTORS_FRAME_.*_ENABLED')
feature_define_whitelist.add('AP_COPTER_ADVANCED_FAILSAFE_ENABLED')
if target.lower() != "plane":
# only on Plane:

7
Tools/autotest/vehicle_test_suite.py
View File

@ -7939,7 +7939,7 @@ class TestSuite(ABC):
(str(m), channel_field))
return m_value
def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq):
def wait_servo_channel_value(self, channel, value, epsilon=0, timeout=2, comparator=operator.eq):
"""wait for channel value comparison (default condition is equality)"""
channel_field = "servo%u_raw" % channel
opstring = ("%s" % comparator)[-3:-1]
@ -7957,8 +7957,11 @@ class TestSuite(ABC):
if m_value is None:
raise ValueError("message (%s) has no field %s" %
(str(m), channel_field))
self.progress("want SERVO_OUTPUT_RAW.%s=%u %s %u" %
self.progress("SERVO_OUTPUT_RAW.%s got=%u %s want=%u" %
(channel_field, m_value, opstring, value))
if comparator == operator.eq:
if abs(m_value - value) <= epsilon:
return m_value
if comparator(m_value, value):
return m_value

2
Tools/environment_install/install-prereqs-ubuntu.sh
View File

@ -162,7 +162,7 @@ else
fi
# Lists of packages to install
BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect"
BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect astyle"
PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy geocoder empy==3.3.4 ptyprocess dronecan"
PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto"

197
Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py Normal file
View File

@ -0,0 +1,197 @@
# Copyright 2023 ArduPilot.org.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""
Bring up ArduPilot SITL and check that the get/set_parameters services are up and running.
Checks whether a parameter is changed using service call.
colcon test --packages-select ardupilot_dds_tests \
--event-handlers=console_cohesion+ --pytest-args -k test_parameter_service
"""
import launch_pytest
import pytest
import rclpy
import rclpy.node
import threading
import time
from launch import LaunchDescription
from launch_pytest.tools import process as process_tools
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rcl_interfaces.srv import GetParameters
from rcl_interfaces.srv import SetParameters
from rcl_interfaces.msg import Parameter
# Enums for parameter type
PARAMETER_NOT_SET = 0
PARAMETER_INTEGER = 2
PARAMETER_DOUBLE = 3
class ParameterClient(rclpy.node.Node):
"""Send GetParameters and SetParameters Requests."""
def __init__(self):
"""Initialize the node."""
super().__init__('parameter_client')
self.get_param_event_object = threading.Event()
self.set_param_event_object = threading.Event()
self.set_correct_object = threading.Event()
def start_client(self):
"""Start the parameter client."""
qos_profile = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
)
# Define clients for getting and setting parameter
self.get_cli = self.create_client(GetParameters, 'ap/get_parameters')
while not self.get_cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info('GetParameters service not available, waiting again...')
self.set_cli = self.create_client(SetParameters, 'ap/set_parameters')
while not self.set_cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info('SetParameters service not availabel, waiting again...')
# Add a spin thread
self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
self.ros_spin_thread.start()
def send_get_param_req(self, param_name):
self.get_req = GetParameters.Request()
self.get_req.names.append(param_name)
self.get_future = self.get_cli.call_async(self.get_req)
while not self.get_future.done():
self.get_logger().info("Waiting for GetParameters service response...")
time.sleep(0.1)
resp = self.get_future.result()
value = resp.values[0]
if value.type != PARAMETER_NOT_SET:
self.get_param_event_object.set()
def send_set_param_req(self, param_name, param_value, param_type):
self.set_req = SetParameters.Request()
param_update = Parameter()
param_update.name = param_name
param_update.value.type = param_type
if param_type == PARAMETER_INTEGER:
param_update.value.integer_value = int(param_value)
else:
param_update.value.double_value = float(param_value)
self.set_req.parameters.append(param_update)
self.desired_value = param_value
get_response = self.get_future.result()
initial_value = get_response.values[0]
if initial_value.type == PARAMETER_INTEGER:
self.param_value = initial_value.integer_value
elif initial_value.type == PARAMETER_DOUBLE:
self.param_value = initial_value.double_value
else:
self.param_value = 'nan'
self.set_future = self.set_cli.call_async(self.set_req)
while not self.set_future.done():
self.get_logger().info("Waiting for SetParameters service response...")
time.sleep(0.1)
if self.set_future.done():
response = self.set_future.result()
if response.results[0].successful:
self.set_param_event_object.set()
def check_param_change(self):
param_name = self.set_req.parameters[0].name
self.send_get_param_req(param_name)
get_response = self.get_future.result()
new_value = get_response.values[0]
if new_value.type == PARAMETER_INTEGER:
updated_value = new_value.integer_value
elif new_value.type == PARAMETER_DOUBLE:
updated_value = new_value.double_value
else:
updated_value = 'nan'
if updated_value == self.desired_value:
self.set_correct_object.set()
@launch_pytest.fixture
def launch_sitl_copter_dds_udp(sitl_copter_dds_udp):
"""Fixture to create the launch description."""
sitl_ld, sitl_actions = sitl_copter_dds_udp
ld = LaunchDescription(
[
sitl_ld,
launch_pytest.actions.ReadyToTest(),
]
)
actions = sitl_actions
yield ld, actions
@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp)
def test_dds_udp_parameter_services(launch_context, launch_sitl_copter_dds_udp):
"""Test Get/Set parameter services broadcast by AP_DDS."""
_, actions = launch_sitl_copter_dds_udp
micro_ros_agent = actions["micro_ros_agent"].action
mavproxy = actions["mavproxy"].action
sitl = actions["sitl"].action
# Wait for process to start.
process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)
rclpy.init()
try:
node = ParameterClient()
node.start_client()
parameter_name = "LOIT_SPEED"
param_change_value = 1250
param_type = PARAMETER_DOUBLE
node.send_get_param_req(parameter_name)
get_param_received_flag = node.get_param_event_object.wait(timeout=10.0)
assert get_param_received_flag, f"Did not get '{parameter_name}' param."
node.send_set_param_req(parameter_name, param_change_value, param_type)
set_param_received_flag = node.set_param_event_object.wait(timeout=10.0)
assert set_param_received_flag, f"Could not set '{parameter_name}' to '{param_change_value}'"
node.check_param_change()
set_param_changed_flag = node.set_correct_object.wait(timeout=10.0)
assert set_param_changed_flag, f"Did not confirm '{parameter_name}' value change"
finally:
rclpy.shutdown()
yield

1
Tools/ros2/ardupilot_msgs/CMakeLists.txt
View File

@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/GlobalPosition.msg"
"msg/Status.msg"
"srv/ArmMotors.srv"
"srv/ModeSwitch.srv"
DEPENDENCIES geometry_msgs std_msgs

27
Tools/ros2/ardupilot_msgs/msg/Status.msg Normal file
View File

@ -0,0 +1,27 @@
std_msgs/Header header
uint8 APM_ROVER = 1
uint8 APM_ARDUCOPTER = 2
uint8 APM_ARDUPLANE = 3
uint8 APM_ANTENNATRACKER = 4
uint8 APM_UNKNOWN = 5
uint8 APM_REPLAY = 6
uint8 APM_ARDUSUB = 7
uint8 APM_IOFIRMWARE = 8
uint8 APM_AP_PERIPH = 9
uint8 APM_AP_DAL_STANDALONE = 10
uint8 APM_AP_BOOTLOADER = 11
uint8 APM_BLIMP = 12
uint8 APM_HELI = 13
uint8 vehicle_type # From AP_Vehicle_Type.h
bool armed # true if vehicle is armed.
uint8 mode # Vehicle mode, enum depending on vehicle type.
bool flying # True if flying/driving/diving/tracking.
bool external_control # True is external control is enabled.
uint8 FS_RADIO = 21
uint8 FS_BATTERY = 22
uint8 FS_GCS = 23
uint8 FS_EKF = 24
uint8[] failsafe # Array containing all active failsafe.

2
Tools/scripts/build_options.py
View File

@ -156,6 +156,7 @@ BUILD_OPTIONS = [
Feature('Copter', 'MODE_FLOWHOLD', 'MODE_FLOWHOLD_ENABLED', 'Enable Mode Flowhold', 0, "OPTICALFLOW"),
Feature('Copter', 'MODE_FLIP', 'MODE_FLIP_ENABLED', 'Enable Mode Flip', 0, None),
Feature('Copter', 'MODE_BRAKE', 'MODE_BRAKE_ENABLED', 'Enable Mode Brake', 0, None),
Feature('Copter', 'COPTER_ADVANCED_FAILSAFE', 'AP_COPTER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501
Feature('Rover', 'ROVER_ADVANCED_FAILSAFE', 'AP_ROVER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501
@ -363,7 +364,6 @@ BUILD_OPTIONS = [
Feature('MAVLink', 'MAVLINK_VERSION_REQUEST', 'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', 'Enable Old AUTOPILOT_VERSION_REQUEST mesage', 0, None), # noqa
Feature('MAVLink', 'REQUEST_AUTOPILOT_CAPA', 'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', 'Enable Old REQUEST_AUTOPILOT_CAPABILITIES command', 0, None), # noqa
Feature('MAVLink', 'MAV_MSG_RELAY_STATUS', 'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', 'Enable Send RELAY_STATUS message', 0, 'RELAY'), # noqa
Feature('MAVLink', 'MAV_MSG_HIL_GPS', 'AP_MAVLINK_MSG_HIL_GPS_ENABLED', 'Enable Receive HIL_GPS messages', 0, 'MAV'), # noqa
Feature('MAVLink', 'MAV_MSG_MOUNT_CONTROL', 'AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED', 'Enable Deprecated MOUNT_CONTROL message', 0, "MOUNT"), # noqa
Feature('MAVLink', 'MAV_MSG_MOUNT_CONFIGURE', 'AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED', 'Enable Deprecated MOUNT_CONFIGURE message', 0, "MOUNT"), # noqa
Feature('MAVLink', 'AP_MAVLINK_BATTERY2_ENABLED', 'AP_MAVLINK_BATTERY2_ENABLED', 'Enable Send old BATTERY2 message', 0, None), # noqa

1
Tools/scripts/decode_devid.py
View File

@ -104,6 +104,7 @@ imu_types = {
0x3A : "DEVTYPE_INS_ICM42670",
0x3B : "DEVTYPE_INS_ICM45686",
0x3C : "DEVTYPE_INS_SCHA63T",
0x3D : "DEVTYPE_INS_IIM42653",
}
baro_types = {

2
Tools/scripts/extract_features.py
View File

@ -273,11 +273,11 @@ class ExtractFeatures(object):
('AP_CUSTOMROTATIONS_ENABLED', 'AP_CustomRotations::init'),
('AP_OSD_LINK_STATS_EXTENSIONS_ENABLED', r'AP_OSD_Screen::draw_rc_tx_power'),
('HAL_ENABLE_DRONECAN_DRIVERS', r'AP_DroneCAN::init'),
('AP_MAVLINK_MSG_HIL_GPS_ENABLED', r'mavlink_msg_hil_gps_decode'),
('AP_BARO_PROBE_EXTERNAL_I2C_BUSES', r'AP_Baro::_probe_i2c_barometers'),
('AP_RSSI_ENABLED', r'AP_RSSI::init'),
('AP_ROVER_ADVANCED_FAILSAFE_ENABLED', r'Rover::afs_fs_check'),
('AP_COPTER_ADVANCED_FAILSAFE_ENABLED', r'Copter::afs_fs_check'),
('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'),
('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'),

2
Tools/scripts/powr_change.py
View File

@ -70,8 +70,6 @@ class POWRChange(object):
if new_acc_bit_set and not old_acc_bit_set:
line += " ACCFLAGS+%s" % self.bit_description(bit)
elif not new_bit_set and old_bit_set:
line += " ACCFLAGS-%s" % self.bit_description(bit)
if len(line) == 0:
continue

4
libraries/AP_AHRS/AP_AHRS.h
View File

@ -618,9 +618,9 @@ public:
// return current vibration vector for primary IMU
Vector3f get_vibration(void) const;
// return primary accels, for lua
// return primary accels
const Vector3f &get_accel(void) const {
return AP::ins().get_accel();
return AP::ins().get_accel(_get_primary_accel_index());
}
// return primary accel bias. This should be subtracted from

2
libraries/AP_Arming/AP_Arming.cpp
View File

@ -175,7 +175,7 @@ extern AP_IOMCU iomcu;
#endif
#pragma GCC diagnostic push
#if defined (__clang__)
#if defined(__clang_major__) && __clang_major__ >= 14
#pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical"
#endif

8
libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp
View File

@ -63,28 +63,28 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = {
// @Param: FL_FF
// @DisplayName: First order term
// @Description: First order polynomial fit term
// @Range: 0 10
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("FL_FF", 45, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_first_order_coeff, 1),
// @Param: FL_FS
// @DisplayName: Second order term
// @Description: Second order polynomial fit term
// @Range: 0 10
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("FL_FS", 46, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_second_order_coeff, 0),
// @Param: FL_FT
// @DisplayName: Third order term
// @Description: Third order polynomial fit term
// @Range: 0 10
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("FL_FT", 47, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_third_order_coeff, 0),
// @Param: FL_OFF
// @DisplayName: Offset term
// @Description: Offset polynomial fit term
// @Range: 0 10
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0),

284
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -17,6 +17,7 @@
#include <AP_Arming/AP_Arming.h>
# endif // AP_DDS_ARM_SERVER_ENABLED
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>
#if AP_DDS_ARM_SERVER_ENABLED
@ -66,6 +67,9 @@ static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS;
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
static constexpr uint16_t DELAY_PING_MS = 500;
#ifdef AP_DDS_STATUS_PUB_ENABLED
static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS;
#endif // AP_DDS_STATUS_PUB_ENABLED
// Define the subscriber data members, which are static class scope.
// If these are created on the stack in the subscriber,
@ -81,6 +85,17 @@ geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
// Define the parameter server data members, which are static class scope.
// If these are created on the stack, then the AP_DDS_Client::on_request
// frame size is exceeded.
#if AP_DDS_PARAMETER_SERVER_ENABLED
rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {};
rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {};
rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {};
rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {};
rcl_interfaces_msg_Parameter AP_DDS_Client::param {};
#endif
const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
// @Param: _ENABLE
@ -604,6 +619,56 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg)
{
// Fill the new message.
const auto &vehicle = AP::vehicle();
const auto &battery = AP::battery();
msg.vehicle_type = static_cast<uint8_t>(AP::fwversion().vehicle_type);
msg.armed = hal.util->get_soft_armed();
msg.mode = vehicle->get_mode();
msg.flying = vehicle->get_likely_flying();
msg.external_control = true; // Always true for now. To be filled after PR#28429.
uint8_t fs_iter = 0;
msg.failsafe_size = 0;
if (AP_Notify::flags.failsafe_radio) {
msg.failsafe[fs_iter++] = FS_RADIO;
}
if (battery.has_failsafed()) {
msg.failsafe[fs_iter++] = FS_BATTERY;
}
if (AP_Notify::flags.failsafe_gcs) {
msg.failsafe[fs_iter++] = FS_GCS;
}
if (AP_Notify::flags.failsafe_ekf) {
msg.failsafe[fs_iter++] = FS_EKF;
}
msg.failsafe_size = fs_iter;
// Compare with the previous one.
bool is_message_changed {false};
is_message_changed |= (last_status_msg_.flying != msg.flying);
is_message_changed |= (last_status_msg_.armed != msg.armed);
is_message_changed |= (last_status_msg_.mode != msg.mode);
is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type);
is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size);
is_message_changed |= (last_status_msg_.external_control != msg.external_control);
if ( is_message_changed ) {
last_status_msg_.flying = msg.flying;
last_status_msg_.armed = msg.armed;
last_status_msg_.mode = msg.mode;
last_status_msg_.vehicle_type = msg.vehicle_type;
last_status_msg_.failsafe_size = msg.failsafe_size;
last_status_msg_.external_control = msg.external_control;
update_topic(msg.header.stamp);
return true;
} else {
return false;
}
}
#endif // AP_DDS_STATUS_PUB_ENABLED
/*
start the DDS thread
*/
@ -801,6 +866,198 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
break;
}
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
if (deserialize_success == false) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix);
break;
}
if (set_parameter_request.parameters_size > 8U) {
break;
}
// Set parameters and responses for each one requested
set_parameter_response.results_size = set_parameter_request.parameters_size;
for (size_t i = 0; i < set_parameter_request.parameters_size; i++) {
param = set_parameter_request.parameters[i];
enum ap_var_type var_type;
// set parameter
AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE + 1];
strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE);
param_key[AP_MAX_NAME_SIZE] = 0;
// Currently only integer and double value types can be set.
// The following parameter value types are not handled:
// bool, string, byte_array, bool_array, integer_array, double_array and string_array
bool param_isnan = true;
bool param_isinf = true;
float param_value;
switch (param.value.type) {
case PARAMETER_INTEGER: {
param_isnan = isnan(param.value.integer_value);
param_isinf = isinf(param.value.integer_value);
param_value = float(param.value.integer_value);
break;
}
case PARAMETER_DOUBLE: {
param_isnan = isnan(param.value.double_value);
param_isinf = isinf(param.value.double_value);
param_value = float(param.value.double_value);
break;
}
default: {
break;
}
}
// find existing param to get the old value
uint16_t parameter_flags = 0;
vp = AP_Param::find(param_key, &var_type, &parameter_flags);
if (vp == nullptr || param_isnan || param_isinf) {
set_parameter_response.results[i].successful = false;
strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason));
continue;
}
// Add existing parameter checks used in GCS_Param.cpp
if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
// The user can set BRD_OPTIONS to enable set of internal
// parameters, for developer testing or unusual use cases
if (AP_BoardConfig::allow_set_internal_parameters()) {
parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
}
}
if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
set_parameter_response.results[i].successful = false;
strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason));
continue;
}
// Set and save the value if it changed
bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value);
if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) {
AP_Param::invalidate_count();
}
set_parameter_response.results[i].successful = true;
strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason));
}
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id,
.type = UXR_REPLIER_ID
};
const uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U);
uint8_t reply_buffer[reply_size];
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
bool successful_params = true;
for (size_t i = 0; i < set_parameter_response.results_size; i++) {
// Check that all the parameters were set successfully
successful_params &= set_parameter_response.results[i].successful;
}
GCS_SEND_TEXT(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" );
break;
}
case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request);
if (deserialize_success == false) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix);
break;
}
if (get_parameters_request.names_size > 8U) {
break;
}
bool successful_read = true;
get_parameters_response.values_size = get_parameters_request.names_size;
for (size_t i = 0; i < get_parameters_request.names_size; i++) {
enum ap_var_type var_type;
AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE + 1];
strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE);
param_key[AP_MAX_NAME_SIZE] = 0;
vp = AP_Param::find(param_key, &var_type);
if (vp == nullptr) {
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
successful_read &= false;
continue;
}
switch (var_type) {
case AP_PARAM_INT8: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_INT16: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_INT32: {
get_parameters_response.values[i].type = PARAMETER_INTEGER;
get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get();
successful_read &= true;
break;
}
case AP_PARAM_FLOAT: {
get_parameters_response.values[i].type = PARAMETER_DOUBLE;
get_parameters_response.values[i].double_value = vp->cast_to_float(var_type);
successful_read &= true;
break;
}
default: {
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
successful_read &= false;
break;
}
}
}
const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id,
.type = UXR_REPLIER_ID
};
const uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U);
uint8_t reply_buffer[reply_size];
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
ucdrBuffer reply_ub;
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
const bool serialize_success = rcl_interfaces_srv_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response);
if (serialize_success == false) {
break;
}
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND");
break;
}
#endif // AP_DDS_PARAMETER_SERVER_ENABLED
}
}
@ -947,7 +1204,7 @@ bool AP_DDS_Client::create()
.id = 0x01,
.type = UXR_PARTICIPANT_ID
};
const char* participant_name = "ardupilot_dds";
const char* participant_name = AP_DDS_PARTICIPANT_NAME;
const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id,
static_cast<uint16_t>(domain_id), participant_name, UXR_REPLACE);
@ -1257,6 +1514,23 @@ void AP_DDS_Client::write_gps_global_origin_topic()
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
void AP_DDS_Client::write_status_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size);
const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
}
}
}
#endif // AP_DDS_STATUS_PUB_ENABLED
void AP_DDS_Client::update()
{
WITH_SEMAPHORE(csem);
@ -1336,6 +1610,14 @@ void AP_DDS_Client::update()
write_gps_global_origin_topic();
}
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
if (update_topic(status_topic)) {
write_status_topic();
}
last_status_check_time_ms = cur_time_ms;
}
#endif // AP_DDS_STATUS_PUB_ENABLED
status_ok = uxr_run_session_time(&session, 1);
}

31
libraries/AP_DDS/AP_DDS_Client.h
View File

@ -25,6 +25,9 @@
#if AP_DDS_IMU_PUB_ENABLED
#include "sensor_msgs/msg/Imu.h"
#endif // AP_DDS_IMU_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
#include "ardupilot_msgs/msg/Status.h"
#endif // AP_DDS_STATUS_PUB_ENABLED
#if AP_DDS_JOY_SUB_ENABLED
#include "sensor_msgs/msg/Joy.h"
#endif // AP_DDS_JOY_SUB_ENABLED
@ -46,6 +49,14 @@
#if AP_DDS_CLOCK_PUB_ENABLED
#include "rosgraph_msgs/msg/Clock.h"
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
#include "rcl_interfaces/srv/SetParameters.h"
#include "rcl_interfaces/msg/Parameter.h"
#include "rcl_interfaces/msg/SetParametersResult.h"
#include "rcl_interfaces/msg/ParameterValue.h"
#include "rcl_interfaces/msg/ParameterType.h"
#include "rcl_interfaces/srv/GetParameters.h"
#endif //AP_DDS_PARAMETER_SERVER_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Scheduler.h>
@ -175,6 +186,17 @@ private:
static void update_topic(rosgraph_msgs_msg_Clock& msg);
#endif // AP_DDS_CLOCK_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
ardupilot_msgs_msg_Status status_topic;
bool update_topic(ardupilot_msgs_msg_Status& msg);
// The last ms timestamp AP_DDS wrote/checked a status message
uint64_t last_status_check_time_ms;
// last status values;
ardupilot_msgs_msg_Status last_status_msg_;
//! @brief Serialize the current status and publish to the IO stream(s)
void write_status_topic();
#endif // AP_DDS_STATUS_PUB_ENABLED
#if AP_DDS_STATIC_TF_PUB_ENABLED
// outgoing transforms
tf2_msgs_msg_TFMessage tx_static_transforms_topic;
@ -201,6 +223,14 @@ private:
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
HAL_Semaphore csem;
#if AP_DDS_PARAMETER_SERVER_ENABLED
static rcl_interfaces_srv_SetParameters_Request set_parameter_request;
static rcl_interfaces_srv_SetParameters_Response set_parameter_response;
static rcl_interfaces_srv_GetParameters_Request get_parameters_request;
static rcl_interfaces_srv_GetParameters_Response get_parameters_response;
static rcl_interfaces_msg_Parameter param;
#endif
// connection parametrics
bool status_ok{false};
bool connected{false};
@ -255,6 +285,7 @@ private:
// client key we present
static constexpr uint32_t key = 0xAAAABBBB;
public:
~AP_DDS_Client();

28
libraries/AP_DDS/AP_DDS_Service_Table.h
View File

@ -6,8 +6,12 @@ enum class ServiceIndex: uint8_t {
ARMING_MOTORS,
#endif // #if AP_DDS_ARM_SERVER_ENABLED
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
MODE_SWITCH
MODE_SWITCH,
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
SET_PARAMETERS,
GET_PARAMETERS
#endif // AP_DDS_PARAMETER_SERVER_ENABLED
};
static inline constexpr uint8_t to_underlying(const ServiceIndex index)
@ -47,4 +51,26 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
.reply_topic_name = "rr/ap/mode_switchReply",
},
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::SET_PARAMETERS),
.rep_id = to_underlying(ServiceIndex::SET_PARAMETERS),
.service_rr = Service_rr::Replier,
.service_name = "rs/ap/set_parametersService",
.request_type = "rcl_interfaces::srv::dds_::SetParameters_Request_",
.reply_type = "rcl_interfaces::srv::dds_::SetParameters_Response_",
.request_topic_name = "rq/ap/set_parametersRequest",
.reply_topic_name = "rr/ap/set_parametersReply",
},
{
.req_id = to_underlying(ServiceIndex::GET_PARAMETERS),
.rep_id = to_underlying(ServiceIndex::GET_PARAMETERS),
.service_rr = Service_rr::Replier,
.service_name = "rs/ap/get_parameterService",
.request_type = "rcl_interfaces::srv::dds_::GetParameters_Request_",
.reply_type = "rcl_interfaces::srv::dds_::GetParameters_Response_",
.request_topic_name = "rq/ap/get_parametersRequest",
.reply_topic_name = "rr/ap/get_parametersReply",
},
#endif // AP_DDS_PARAMETER_SERVER_ENABLED
};

21
libraries/AP_DDS/AP_DDS_Topic_Table.h
View File

@ -48,6 +48,9 @@ enum class TopicIndex: uint8_t {
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
GPS_GLOBAL_ORIGIN_PUB,
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
STATUS_PUB,
#endif // AP_DDS_STATUS_PUB_ENABLED
#if AP_DDS_JOY_SUB_ENABLED
JOY_SUB,
#endif // AP_DDS_JOY_SUB_ENABLED
@ -268,6 +271,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
},
},
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
#if AP_DDS_STATUS_PUB_ENABLED
{
.topic_id = to_underlying(TopicIndex::STATUS_PUB),
.pub_id = to_underlying(TopicIndex::STATUS_PUB),
.sub_id = to_underlying(TopicIndex::STATUS_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAREADER_ID},
.topic_rw = Topic_rw::DataWriter,
.topic_name = "rt/ap/status",
.type_name = "ardupilot_msgs::msg::dds_::Status_",
.qos = {
.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
.reliability = UXR_RELIABILITY_RELIABLE,
.history = UXR_HISTORY_KEEP_LAST,
.depth = 1,
},
},
#endif // AP_DDS_STATUS_PUB_ENABLED
#if AP_DDS_JOY_SUB_ENABLED
{
.topic_id = to_underlying(TopicIndex::JOY_SUB),

16
libraries/AP_DDS/AP_DDS_config.h
View File

@ -94,6 +94,10 @@
#define AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS 1000
#endif
#ifndef AP_DDS_DELAY_STATUS_TOPIC_MS
#define AP_DDS_DELAY_STATUS_TOPIC_MS 100
#endif
#ifndef AP_DDS_CLOCK_PUB_ENABLED
#define AP_DDS_CLOCK_PUB_ENABLED 1
#endif
@ -102,6 +106,10 @@
#define AP_DDS_DELAY_CLOCK_TOPIC_MS 10
#endif
#ifndef AP_DDS_STATUS_PUB_ENABLED
#define AP_DDS_STATUS_PUB_ENABLED 1
#endif
#ifndef AP_DDS_JOY_SUB_ENABLED
#define AP_DDS_JOY_SUB_ENABLED 1
#endif
@ -126,6 +134,10 @@
#define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1
#endif
#ifndef AP_DDS_PARAMETER_SERVER_ENABLED
#define AP_DDS_PARAMETER_SERVER_ENABLED 1
#endif
// Whether to include Twist support
#define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED
@ -139,3 +151,7 @@
#define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1"
#endif
#endif
#ifndef AP_DDS_PARTICIPANT_NAME
#define AP_DDS_PARTICIPANT_NAME "ap"
#endif

56
libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl Normal file
View File

@ -0,0 +1,56 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from ardupilot_msgs/msg/Status.msg
// generated code does not contain a copyright notice
#include "std_msgs/msg/Header.idl"
module ardupilot_msgs {
module msg {
module Status_Constants {
const uint8 APM_ROVER = 1;
const uint8 APM_ARDUCOPTER = 2;
const uint8 APM_ARDUPLANE = 3;
const uint8 APM_ANTENNATRACKER = 4;
const uint8 APM_UNKNOWN = 5;
const uint8 APM_REPLAY = 6;
const uint8 APM_ARDUSUB = 7;
const uint8 APM_IOFIRMWARE = 8;
const uint8 APM_AP_PERIPH = 9;
const uint8 APM_AP_DAL_STANDALONE = 10;
const uint8 APM_AP_BOOTLOADER = 11;
const uint8 APM_BLIMP = 12;
const uint8 APM_HELI = 13;
const uint8 FS_RADIO = 21;
const uint8 FS_BATTERY = 22;
const uint8 FS_GCS = 23;
const uint8 FS_EKF = 24;
};
struct Status {
std_msgs::msg::Header header;
@verbatim (language="comment", text=
"From AP_Vehicle_Type.h")
uint8 vehicle_type;
@verbatim (language="comment", text=
"true if vehicle is armed.")
boolean armed;
@verbatim (language="comment", text=
"Vehicle mode, enum depending on vehicle type.")
uint8 mode;
@verbatim (language="comment", text=
"True if flying/driving/diving/tracking.")
boolean flying;
@verbatim (language="comment", text=
"True is external control is enabled.")
boolean external_control;
@verbatim (language="comment", text=
"Array containing all active failsafe.")
sequence<uint8> failsafe;
};
};
};

23
libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl Normal file
View File

@ -0,0 +1,23 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from rcl_interfaces/msg/Parameter.msg
// generated code does not contain a copyright notice
#include "rcl_interfaces/msg/ParameterValue.idl"
module rcl_interfaces {
module msg {
@verbatim (language="comment", text=
"This is the message to communicate a parameter. It is an open struct with an enum in" "\n"
"the descriptor to select which value is active.")
struct Parameter {
@verbatim (language="comment", text=
"The full name of the parameter.")
string name;
@verbatim (language="comment", text=
"The parameter's value which can be one of several types, see" "\n"
"`ParameterValue.msg` and `ParameterType.msg`.")
rcl_interfaces::msg::ParameterValue value;
};
};
};

28
libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl Normal file
View File

@ -0,0 +1,28 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from rcl_interfaces/msg/ParameterType.msg
// generated code does not contain a copyright notice
module rcl_interfaces {
module msg {
module ParameterType_Constants {
@verbatim (language="comment", text=
"Default value, which implies this is not a valid parameter.")
const uint8 PARAMETER_NOT_SET = 0;
const uint8 PARAMETER_BOOL = 1;
const uint8 PARAMETER_INTEGER = 2;
const uint8 PARAMETER_DOUBLE = 3;
const uint8 PARAMETER_STRING = 4;
const uint8 PARAMETER_BYTE_ARRAY = 5;
const uint8 PARAMETER_BOOL_ARRAY = 6;
const uint8 PARAMETER_INTEGER_ARRAY = 7;
const uint8 PARAMETER_DOUBLE_ARRAY = 8;
const uint8 PARAMETER_STRING_ARRAY = 9;
};
@verbatim (language="comment", text=
"These types correspond to the value that is set in the ParameterValue message.")
struct ParameterType {
uint8 structure_needs_at_least_one_member;
};
};
};

58
libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl Normal file
View File

@ -0,0 +1,58 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from rcl_interfaces/msg/ParameterValue.msg
// generated code does not contain a copyright notice
module rcl_interfaces {
module msg {
@verbatim (language="comment", text=
"Used to determine which of the next *_value fields are set." "\n"
"ParameterType.PARAMETER_NOT_SET indicates that the parameter was not set" "\n"
"(if gotten) or is uninitialized." "\n"
"Values are enumerated in `ParameterType.msg`.")
struct ParameterValue {
@verbatim (language="comment", text=
"The type of this parameter, which corresponds to the appropriate field below.")
uint8 type;
@verbatim (language="comment", text=
"\"Variant\" style storage of the parameter value. Only the value corresponding" "\n"
"the type field will have valid information." "\n"
"Boolean value, can be either true or false.")
boolean bool_value;
@verbatim (language="comment", text=
"Integer value ranging from -9,223,372,036,854,775,808 to" "\n"
"9,223,372,036,854,775,807.")
int64 integer_value;
@verbatim (language="comment", text=
"A double precision floating point value following IEEE 754.")
double double_value;
@verbatim (language="comment", text=
"A textual value with no practical length limit.")
string string_value;
@verbatim (language="comment", text=
"An array of bytes, used for non-textual information.")
sequence<octet> byte_array_value;
@verbatim (language="comment", text=
"An array of boolean values.")
sequence<boolean> bool_array_value;
@verbatim (language="comment", text=
"An array of 64-bit integer values.")
sequence<int64> integer_array_value;
@verbatim (language="comment", text=
"An array of 64-bit floating point values.")
sequence<double> double_array_value;
@verbatim (language="comment", text=
"An array of string values.")
sequence<string> string_array_value;
};
};
};

20
libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl Normal file
View File

@ -0,0 +1,20 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from rcl_interfaces/msg/SetParametersResult.msg
// generated code does not contain a copyright notice
module rcl_interfaces {
module msg {
@verbatim (language="comment", text=
"A true value of the same index indicates that the parameter was set" "\n"
"successfully. A false value indicates the change was rejected.")
struct SetParametersResult {
boolean successful;
@verbatim (language="comment", text=
"Reason why the setting was either successful or a failure. This should only be" "\n"
"used for logging and user interfaces.")
string reason;
};
};
};

29
libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl Normal file
View File

@ -0,0 +1,29 @@
// generated from rosidl_adapter/resource/srv.idl.em
// with input from rcl_interfaces/srv/GetParameters.srv
// generated code does not contain a copyright notice
#include "rcl_interfaces/msg/ParameterValue.idl"
module rcl_interfaces {
module srv {
@verbatim (language="comment", text=
"TODO(wjwwood): Decide on the rules for grouping, nodes, and parameter \"names\"" "\n"
"in general, then link to that." "\n"
"" "\n"
"For more information about parameters and naming rules, see:" "\n"
"https://design.ros2.org/articles/ros_parameters.html" "\n"
"https://github.com/ros2/design/pull/241")
struct GetParameters_Request {
@verbatim (language="comment", text=
"A list of parameter names to get.")
sequence<string> names;
};
@verbatim (language="comment", text=
"List of values which is the same length and order as the provided names. If a" "\n"
"parameter was not yet set, the value will have PARAMETER_NOT_SET as the" "\n"
"type.")
struct GetParameters_Response {
sequence<rcl_interfaces::msg::ParameterValue> values;
};
};
};

21
libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl Normal file
View File

@ -0,0 +1,21 @@
// generated from rosidl_adapter/resource/srv.idl.em
// with input from rcl_interfaces/srv/SetParameters.srv
// generated code does not contain a copyright notice
#include "rcl_interfaces/msg/Parameter.idl"
#include "rcl_interfaces/msg/SetParametersResult.idl"
module rcl_interfaces {
module srv {
@verbatim (language="comment", text=
"A list of parameters to set.")
struct SetParameters_Request {
sequence<rcl_interfaces::msg::Parameter> parameters;
};
@verbatim (language="comment", text=
"Indicates whether setting each parameter succeeded or not and why.")
struct SetParameters_Response {
sequence<rcl_interfaces::msg::SetParametersResult> results;
};
};
};

8
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1367,6 +1367,7 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const
return yaw_cd;
}
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
const Location &loc = location(0);
@ -1400,8 +1401,9 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
0, // TODO one-sigma heading accuracy standard deviation
gps_yaw_cdeg(0));
}
#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED
#if GPS_MAX_RECEIVERS > 1
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
// always send the message if 2nd GPS is configured
@ -1442,9 +1444,9 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
sacc * 1000, // one-sigma standard deviation in mm/s
0); // TODO one-sigma heading accuracy standard deviation
}
#endif // GPS_MAX_RECEIVERS
#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED
#if HAL_GCS_ENABLED
#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
{
if (inst >= GPS_MAX_RECEIVERS) {

45
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -143,51 +143,8 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
state.last_gps_time_ms = now_ms;
_new_data = true;
break;
}
}
#if AP_MAVLINK_MSG_HIL_GPS_ENABLED
case MAVLINK_MSG_ID_HIL_GPS: {
mavlink_hil_gps_t packet;
mavlink_msg_hil_gps_decode(&msg, &packet);
// check if target instance belongs to incoming gps data.
if (state.instance != packet.id) {
return;
}
state.time_week = 0;
state.time_week_ms = packet.time_usec/1000;
state.status = (AP_GPS::GPS_Status)packet.fix_type;
Location loc = {};
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt * 0.1f;
state.location = loc;
state.hdop = MIN(packet.eph, GPS_UNKNOWN_DOP);
state.vdop = MIN(packet.epv, GPS_UNKNOWN_DOP);
if (packet.vel < 65535) {
state.ground_speed = packet.vel * 0.01f;
}
Vector3f vel(packet.vn*0.01f, packet.ve*0.01f, packet.vd*0.01f);
state.velocity = vel;
if (packet.vd != 0) {
state.have_vertical_velocity = true;
}
if (packet.cog < 36000) {
state.ground_course = packet.cog * 0.01f;
}
state.have_speed_accuracy = false;
state.have_horizontal_accuracy = false;
state.have_vertical_accuracy = false;
if (packet.satellites_visible < 255) {
state.num_sats = packet.satellites_visible;
}
state.last_gps_time_ms = AP_HAL::millis();
_new_data = true;
break;
}
#endif // AP_MAVLINK_MSG_HIL_GPS_ENABLED
default:
// ignore all other messages
break;

17
libraries/AP_GPS/AP_GPS_config.h
View File

@ -104,3 +104,20 @@
#ifndef HAL_GPS_COM_PORT_DEFAULT
#define HAL_GPS_COM_PORT_DEFAULT 1
#endif
#ifndef AP_GPS_GPS_RAW_INT_SENDING_ENABLED
#define AP_GPS_GPS_RAW_INT_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED
#endif
#ifndef AP_GPS_GPS2_RAW_SENDING_ENABLED
#define AP_GPS_GPS2_RAW_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1
#endif
#ifndef AP_GPS_GPS_RTK_SENDING_ENABLED
#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED)
#endif
#ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED
#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED)
#endif

5
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -172,7 +172,7 @@ bool AP_GPS_Backend::should_log() const
#endif
#if HAL_GCS_ENABLED
#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
const uint8_t instance = state.instance;
@ -212,7 +212,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
break;
}
}
#endif
#endif // AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
/*

2
libraries/AP_GPS/GPS_Backend.h
View File

@ -71,7 +71,9 @@ public:
#if HAL_GCS_ENABLED
//MAVLink methods
virtual bool supports_mavlink_gps_rtk_message() const { return false; }
#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
#endif
virtual void handle_msg(const mavlink_message_t &msg) { return ; }
#endif

4
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -6,6 +6,7 @@
*/
#pragma once
// @LoggerEnum: HAL_BOARD
#define HAL_BOARD_SITL 3
// #define HAL_BOARD_SMACCM 4 // unused
// #define HAL_BOARD_PX4 5 // unused
@ -16,7 +17,9 @@
#define HAL_BOARD_ESP32 12
#define HAL_BOARD_QURT 13
#define HAL_BOARD_EMPTY 99
// @LoggerEnumEnd
// @LoggerEnum: HAL_BOARD_SUBTYPE
/* Default board subtype is -1 */
#define HAL_BOARD_SUBTYPE_NONE -1
@ -70,6 +73,7 @@
#define HAL_BOARD_SUBTYPE_ESP32_NICK 6006
#define HAL_BOARD_SUBTYPE_ESP32_S3DEVKIT 6007
#define HAL_BOARD_SUBTYPE_ESP32_S3EMPTY 6008
// @LoggerEnumEnd
/* InertialSensor driver types */
#define HAL_INS_NONE 0

6
libraries/AP_HAL/HAL.h
View File

@ -67,9 +67,6 @@ public:
scheduler(_scheduler),
util(_util),
opticalflow(_opticalflow),
#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
simstate(_simstate),
#endif
flash(_flash),
#if HAL_WITH_DSP
dsp(_dsp),
@ -85,6 +82,9 @@ public:
_serial7,
_serial8,
_serial9}
#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL
,simstate(_simstate)
#endif
{
#if HAL_NUM_CAN_IFACES > 0
if (_can_ifaces == nullptr) {

1
libraries/AP_HAL/board/linux.h
View File

@ -57,6 +57,7 @@
#define HAL_LINUX_HEAT_TARGET_TEMP 50
#define BEBOP_CAMV_PWM 9
#define BEBOP_CAMV_PWM_FREQ 43333333
#define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP
#define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0"
#define HAL_OPTFLOW_ONBOARD_SUBDEV_PATH "/dev/v4l-subdev0"
#define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320

2
libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat
View File

@ -159,7 +159,9 @@ BARO DPS310 I2C:0:0x76
# IMU setup
SPIDEV imu1 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 8*MHZ
SPIDEV imu2 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ
IMU Invensense SPI:imu1 ROTATION_YAW_270
IMU Invensensev3 SPI:imu2 ROTATION_YAW_180
DMA_NOSHARE TIM3_UP TIM4_UP TIM8_UP SPI2*
DMA_PRIORITY TIM3_UP TIM4_UP TIM8_UP SPI2*

1
libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat
View File

@ -130,6 +130,7 @@ define HAL_BARO_20789_I2C_ADDR_ICM 0x68
define HAL_I2C_CLEAR_BUS
define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412
define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART
# the web UI uses an abin file for firmware uploads
env BUILD_ABIN True

2
libraries/AP_HAL_ESP32/AnalogIn.cpp
View File

@ -141,8 +141,8 @@ void adc_calibration_deinit(adc_cali_handle_t handle)
//ardupin is the ardupilot assigned number, starting from 1-8(max)
AnalogSource::AnalogSource(int16_t ardupin, adc_channel_t adc_channel, float scaler, float initial_value) :
_ardupin(ardupin),
_adc_channel(adc_channel),
_ardupin(ardupin),
_scaler(scaler),
_value(initial_value),
_latest_value(initial_value),

2
libraries/AP_HAL_Linux/Flow_PX4.cpp
View File

@ -53,8 +53,8 @@ Flow_PX4::Flow_PX4(uint32_t width, uint32_t bytesperline,
float bottom_flow_feature_threshold,
float bottom_flow_value_threshold) :
_width(width),
_bytesperline(bytesperline),
_search_size(max_flow_pixel),
_bytesperline(bytesperline),
_bottom_flow_feature_threshold(bottom_flow_feature_threshold),
_bottom_flow_value_threshold(bottom_flow_value_threshold)
{

4
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -185,12 +185,14 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
// conditions.
if (speedup > 1 && hal.scheduler->in_main_thread()) {
while (true) {
const int queue_length = ((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length();
HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0);
const int queue_length = uart->get_system_outqueue_length();
// ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length);
if (queue_length < 1024) {
break;
}
_serial_0_outqueue_full_count++;
uart->handle_reading_from_device_to_readbuffer();
usleep(1000);
}
}

3
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -110,9 +110,6 @@ private:
uint32_t wind_start_delay_micros;
uint32_t last_wind_update_us;
// simulated GPS devices
SITL::GPS *gps[2]; // constrained by # of parameter sets
// returns a voltage between 0V to 5V which should appear as the
// voltage from the sensor
float _sonar_pin_voltage() const;

23
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -1208,7 +1208,7 @@ void AP_IOMCU::bind_dsm(uint8_t mode)
setup for mixing. This allows fixed wing aircraft to fly in manual
mode if the FMU dies
*/
bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
bool AP_IOMCU::setup_mixing(int8_t override_chan,
float mixing_gain, uint16_t manual_rc_mask)
{
if (!is_chibios_backend) {
@ -1229,16 +1229,19 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
MIX_UPDATE(mixing.servo_function[i], c->get_function());
MIX_UPDATE(mixing.servo_reversed[i], c->get_reversed());
}
// update RCMap
MIX_UPDATE(mixing.rc_channel[0], rcmap->roll());
MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch());
MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle());
MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw());
auto &xrc = rc();
// note that if not all of these channels are specified correctly
// in parameters then these may be a "dummy" RC channel pointer.
// In that case c->ch() will be zero.
const RC_Channel *channels[] {
&xrc.get_roll_channel(),
&xrc.get_pitch_channel(),
&xrc.get_throttle_channel(),
&xrc.get_yaw_channel()
};
for (uint8_t i=0; i<4; i++) {
const RC_Channel *c = RC_Channels::rc_channel(mixing.rc_channel[i]-1);
if (!c) {
continue;
}
const auto *c = channels[i];
MIX_UPDATE(mixing.rc_channel[i], c->ch());
MIX_UPDATE(mixing.rc_min[i], c->get_radio_min());
MIX_UPDATE(mixing.rc_max[i], c->get_radio_max());
MIX_UPDATE(mixing.rc_trim[i], c->get_radio_trim());

3
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -11,7 +11,6 @@
#if HAL_WITH_IO_MCU
#include "iofirmware/ioprotocol.h"
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_HAL/RCOutput.h>
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
@ -151,7 +150,7 @@ public:
void soft_reboot();
// setup for FMU failsafe mixing
bool setup_mixing(RCMapper *rcmap, int8_t override_chan,
bool setup_mixing(int8_t override_chan,
float mixing_gain, uint16_t manual_rc_mask);
// Check if pin number is valid and configured for GPIO

Some files were not shown because too many files have changed in this diff Show More