diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 48048dc1a17cdc..b1adb3ba11af27 100644 --- a/AntennaTracker/Log.cpp +++ b/AntennaTracker/Log.cpp @@ -7,9 +7,7 @@ // Write an attitude packet void Tracker::Log_Write_Attitude() { - Vector3f targets; - targets.y = nav_status.pitch * 100.0f; - targets.z = wrap_360_cd(nav_status.bearing * 100.0f); + const Vector3f targets{0.0f, nav_status.pitch, nav_status.bearing}; ahrs.Write_Attitude(targets); AP::ahrs().Log_Write(); } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 63f238f2d7612c..cb07bd3d9a2f18 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -75,11 +75,9 @@ void Copter::Log_Write_Control_Tuning() // Write an attitude packet void Copter::Log_Write_Attitude() { - Vector3f targets = attitude_control->get_att_target_euler_cd(); - targets.z = wrap_360_cd(targets.z); - ahrs.Write_Attitude(targets); - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); - } + ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG); + attitude_control->Write_Rate(*pos_control); +} // Write PIDS packets void Copter::Log_Write_PIDS() diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 4c345a5ecaaae7..77d72c7073bdce 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -15,7 +15,7 @@ class _AutoTakeoff { public: void run(); void start(float complete_alt_cm, bool terrain_alt); - bool get_position(Vector3p& completion_pos); + bool get_completion_pos(Vector3p& pos_neu_cm); bool complete; // true when takeoff is complete diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f6cdec65603f78..d95687bf12df2d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -427,7 +427,7 @@ bool ModeAuto::wp_start(const Location& dest_loc) Vector3f stopping_point; if (_mode == SubMode::TAKEOFF) { Vector3p takeoff_complete_pos; - if (auto_takeoff.get_position(takeoff_complete_pos)) { + if (auto_takeoff.get_completion_pos(takeoff_complete_pos)) { stopping_point = takeoff_complete_pos.tofloat(); } } diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 49eeee371c90c7..b92178d0981ea2 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -20,10 +20,11 @@ void Copter::default_dead_zones() void Copter::init_rc_in() { - channel_roll = rc().channel(rcmap.roll()-1); - channel_pitch = rc().channel(rcmap.pitch()-1); - channel_throttle = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + // the library gaurantees that these are non-nullptr: + channel_roll = &rc().get_roll_channel(); + channel_pitch = &rc().get_pitch_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 14fb7d39da7ba0..e31ea1871001ec 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -239,15 +239,15 @@ void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt) } } -// return takeoff final position if takeoff has completed successfully -bool _AutoTakeoff::get_position(Vector3p& _complete_pos) +// return takeoff final target position in cm from the EKF origin if takeoff has completed successfully +bool _AutoTakeoff::get_completion_pos(Vector3p& pos_neu_cm) { // only provide location if takeoff has completed if (!complete) { return false; } - complete_pos = _complete_pos; + pos_neu_cm = complete_pos; return true; } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e523b3ea04b804..137a895f2e326b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -216,6 +216,17 @@ void Plane::update_speed_height(void) should_run_tecs = false; } #endif + + if (auto_state.idle_mode) { + should_run_tecs = false; + } + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (mode_auto.in_pullup()) { + should_run_tecs = false; + } +#endif + if (should_run_tecs) { // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for @@ -512,6 +523,12 @@ void Plane::update_fly_forward(void) } #endif + if (auto_state.idle_mode) { + // don't fuse airspeed when in balloon lift + ahrs.set_fly_forward(false); + return; + } + if (flight_stage == AP_FixedWing::FlightStage::LAND) { ahrs.set_fly_forward(landing.is_flying_forward()); return; @@ -578,6 +595,16 @@ void Plane::update_alt() should_run_tecs = false; } #endif + + if (auto_state.idle_mode) { + should_run_tecs = false; + } + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (mode_auto.in_pullup()) { + should_run_tecs = false; + } +#endif if (should_run_tecs && !throttle_suppressed) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767c..b06bb99cb655f4 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -5,10 +5,11 @@ // Write an attitude packet void Plane::Log_Write_Attitude(void) { - Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude - targets.x = nav_roll_cd; - targets.y = nav_pitch_cd; - targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder. + Vector3f targets { // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude + nav_roll_cd * 0.01f, + nav_pitch_cd * 0.01f, + 0 //Plane does not have the concept of navyaw. This is a placeholder. + }; #if HAL_QUADPLANE_ENABLED if (quadplane.show_vtol_view()) { @@ -18,8 +19,7 @@ void Plane::Log_Write_Attitude(void) // since Euler angles are not used and it is a waste of cpu to compute them at the loop rate. // Get them from the quaternion instead: quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z); - targets *= degrees(100.0f); - quadplane.ahrs_view->Write_AttitudeView(targets); + quadplane.ahrs_view->Write_AttitudeView(targets * RAD_TO_DEG); } else #endif { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 2fb494f1a6b5e1..40148509f714de 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1019,6 +1019,12 @@ const AP_Param::Info Plane::var_info[] = { // @Path: mode_takeoff.cpp GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff), +#if AP_PLANE_GLIDER_PULLUP_ENABLED + // @Group: PUP_ + // @Path: pullup.cpp + GOBJECTN(mode_auto.pullup, pullup, "PUP_", GliderPullup), +#endif + // @Group: // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp PARAM_VEHICLE_INFO, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 38801d99631cc2..612de354fc8c12 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,8 @@ class Parameters { k_param_autotune_options, k_param_takeoff_throttle_min, k_param_takeoff_options, + + k_param_pullup = 270, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..9978b5f2eac0ba 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -121,6 +121,7 @@ #include "avoidance_adsb.h" #endif #include "AP_Arming.h" +#include "pullup.h" /* main APM:Plane class @@ -175,6 +176,9 @@ class Plane : public AP_Vehicle { #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Plane; #endif +#if AP_PLANE_GLIDER_PULLUP_ENABLED + friend class GliderPullup; +#endif Plane(void); @@ -515,6 +519,11 @@ class Plane : public AP_Vehicle { // have we checked for an auto-land? bool checked_for_autoland; + // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters + // are we in idle mode? used for balloon launch to stop servo + // movement until altitude is reached + bool idle_mode; + // are we in VTOL mode in AUTO? bool vtol_mode; @@ -959,6 +968,7 @@ class Plane : public AP_Vehicle { void do_loiter_turns(const AP_Mission::Mission_Command& cmd); void do_loiter_time(const AP_Mission::Mission_Command& cmd); void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); + void do_altitude_wait(const AP_Mission::Mission_Command& cmd); void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); void do_vtol_land(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 9fff6923ef9c1c..6309834ac56578 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -27,6 +27,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) nav_controller->set_data_is_stale(); + // start non-idle + auto_state.idle_mode = false; + // reset loiter start time. New command is a new loiter loiter.start_time_ms = 0; @@ -92,6 +95,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_ALTITUDE_WAIT: + do_altitude_wait(cmd); break; #if HAL_QUADPLANE_ENABLED @@ -501,6 +505,15 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) reset_offset_altitude(); } +void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd) +{ + // set all servos to trim until we reach altitude or descent speed + auto_state.idle_mode = true; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + mode_auto.pullup.reset(); +#endif +} + void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { //set target alt @@ -831,17 +844,46 @@ bool Plane::verify_continue_and_change_alt() */ bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) { - if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) { - gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude"); - return true; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.in_pullup()) { + return pullup.verify_pullup(); } - if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { +#endif + + /* + the target altitude in param1 is always AMSL + */ + const float alt_diff = plane.current_loc.alt*0.01 - cmd.content.altitude_wait.altitude; + bool completed = false; + if (alt_diff > 0) { + gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude"); + completed = true; + } else if (cmd.content.altitude_wait.descent_rate > 0 && + plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate); - return true; + completed = true; } - // if requested, wiggle servos - if (cmd.content.altitude_wait.wiggle_time != 0) { + if (completed) { +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.pullup_start()) { + // we are doing a pullup, ALTITUDE_WAIT not complete until pullup is done + return false; + } +#endif + return true; + } + + const float time_to_alt = alt_diff / MIN(plane.auto_state.sink_rate, -0.01); + + /* + if requested, wiggle servos + + we don't start a wiggle if we expect to release soon as we don't + want the servos to be off trim at the time of release + */ + if (cmd.content.altitude_wait.wiggle_time != 0 && + (plane.auto_state.sink_rate > 0 || time_to_alt > cmd.content.altitude_wait.wiggle_time*5)) { if (wiggle.stage == 0 && AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) { wiggle.stage = 1; @@ -1291,3 +1333,4 @@ bool Plane::in_auto_mission_id(uint16_t command) const { return control_mode == &mode_auto && mission.get_current_nav_id() == command; } + diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a74d7c1a9367a0..f442bd830cf846 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -101,6 +101,9 @@ bool Mode::enter() plane.target_altitude.terrain_following_pending = false; #endif + // disable auto mode servo idle during altitude wait command + plane.auto_state.idle_mode = false; + bool enter_result = _enter(); if (enter_result) { diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 955a66bbc8b065..c26a1233a87b73 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -9,6 +9,7 @@ #include "quadplane.h" #include #include +#include "pullup.h" class AC_PosControl; class AC_AttitudeControl_Multi; @@ -208,6 +209,7 @@ friend class ModeQAcro; class ModeAuto : public Mode { public: + friend class Plane; Number mode_number() const override { return Number::AUTO; } const char *name() const override { return "AUTO"; } @@ -237,6 +239,10 @@ class ModeAuto : public Mode void run() override; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + bool in_pullup() const { return pullup.in_pullup(); } +#endif + protected: bool _enter() override; @@ -257,6 +263,10 @@ class ModeAuto : public Mode uint8_t stage; uint32_t last_ms; } wiggle; + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + GliderPullup pullup; +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED }; diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 3153d7de31e069..b0d0a0585d9e4c 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -79,6 +79,12 @@ void ModeAuto::update() } #endif +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.in_pullup()) { + return; + } +#endif + if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { plane.takeoff_calc_roll(); @@ -166,6 +172,13 @@ bool ModeAuto::is_landing() const void ModeAuto::run() { +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.in_pullup()) { + pullup.stabilize_pullup(); + return; + } +#endif + if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) { wiggle_servos(); diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index e3297f894334e8..b47160836aff84 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -321,7 +321,7 @@ void Plane::update_loiter_update_nav(uint16_t radius) if ((loiter.start_time_ms == 0 && (control_mode == &mode_auto || control_mode == &mode_guided) && auto_state.crosstrack && - current_loc.get_distance(next_WP_loc) > radius*3) || + current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) || quadplane_qrtl_switch) { /* if never reached loiter point and using crosstrack and somewhat far away from loiter point diff --git a/ArduPlane/pullup.cpp b/ArduPlane/pullup.cpp new file mode 100644 index 00000000000000..1f91ca0ac2e52e --- /dev/null +++ b/ArduPlane/pullup.cpp @@ -0,0 +1,239 @@ +#include "Plane.h" +/* + support for pullup after an alitude wait. Used for high altitude gliders + */ + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + +// Pullup control parameters +const AP_Param::GroupInfo GliderPullup::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable pullup after altitude wait + // @Description: Enable pullup after altitude wait + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, GliderPullup, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: ELEV_OFS + // @DisplayName: Elevator deflection used before starting pullup + // @Description: Elevator deflection offset from -1 to 1 while waiting for airspeed to rise before starting close loop control of the pullup. + // @Range: -1.0 1.0 + // @User: Advanced + AP_GROUPINFO("ELEV_OFS", 2, GliderPullup, elev_offset, 0.1f), + + // @Param: NG_LIM + // @DisplayName: Maximum normal load factor during pullup + // @Description: This is the nominal maximum value of normal load factor used during the closed loop pitch rate control of the pullup. + // @Range: 1.0 4.0 + // @User: Advanced + AP_GROUPINFO("NG_LIM", 3, GliderPullup, ng_limit, 2.0f), + + // @Param: NG_JERK_LIM + // @DisplayName: Maximum normal load factor rate of change during pullup + // @Description: The normal load factor used for closed loop pitch rate control of the pullup will be ramped up to the value set by PUP_NG_LIM at the rate of change set by this parameter. The parameter value specified will be scaled internally by 1/EAS2TAS. + // @Units: 1/s + // @Range: 0.1 10.0 + // @User: Advanced + AP_GROUPINFO("NG_JERK_LIM", 4, GliderPullup, ng_jerk_limit, 4.0f), + + // @Param: PITCH + // @DisplayName: Target pitch angle during pullup + // @Description: The vehicle will attempt achieve this pitch angle during the pull-up maneouvre. + // @Units: deg + // @Range: -5 15 + // @User: Advanced + AP_GROUPINFO("PITCH", 5, GliderPullup, pitch_dem, 3), + + // @Param: ARSPD_START + // @DisplayName: Pullup target airspeed + // @Description: Target airspeed for initial airspeed wait + // @Units: m/s + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("ARSPD_START", 6, GliderPullup, airspeed_start, 30), + + // @Param: PITCH_START + // @DisplayName: Pullup target pitch + // @Description: Target pitch for initial pullup + // @Units: deg + // @Range: -80 0 + // @User: Advanced + AP_GROUPINFO("PITCH_START", 7, GliderPullup, pitch_start, -60), + + AP_GROUPEND +}; + +// constructor +GliderPullup::GliderPullup(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + return true if in a pullup manoeuvre at the end of NAV_ALTITUDE_WAIT +*/ +bool GliderPullup::in_pullup(void) const +{ + return plane.control_mode == &plane.mode_auto && + plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT && + stage != Stage::NONE; +} + +/* + start a pullup maneouvre, called when NAV_ALTITUDE_WAIT has reached + altitude or exceeded descent rate +*/ +bool GliderPullup::pullup_start(void) +{ + if (enable != 1) { + return false; + } + + // release balloon + SRV_Channels::set_output_scaled(SRV_Channel::k_lift_release, 100); + + stage = Stage::WAIT_AIRSPEED; + plane.auto_state.idle_mode = false; + float aspeed; + if (!plane.ahrs.airspeed_estimate(aspeed)) { + aspeed = -1; + } + gcs().send_text(MAV_SEVERITY_INFO, "Start pullup airspeed %.1fm/s at %.1fm AMSL", aspeed, plane.current_loc.alt*0.01); + return true; +} + +/* + first stage pullup from balloon release, verify completion + */ +bool GliderPullup::verify_pullup(void) +{ + const auto &ahrs = plane.ahrs; + const auto ¤t_loc = plane.current_loc; + const auto &aparm = plane.aparm; + + switch (stage) { + case Stage::WAIT_AIRSPEED: { + float aspeed; + if (ahrs.airspeed_estimate(aspeed) && aspeed > airspeed_start) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01); + stage = Stage::WAIT_PITCH; + } + return false; + } + + case Stage::WAIT_PITCH: { + if (ahrs.pitch_sensor*0.01 > pitch_start && fabsf(ahrs.roll_sensor*0.01) < 90) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup pitch p=%.1f r=%.1f alt %.1fm AMSL", + ahrs.pitch_sensor*0.01, + ahrs.roll_sensor*0.01, + current_loc.alt*0.01); + stage = Stage::WAIT_LEVEL; + } + return false; + } + + case Stage::PUSH_NOSE_DOWN: { + if (fabsf(ahrs.roll_sensor*0.01) < aparm.roll_limit) { + stage = Stage::WAIT_LEVEL; + } + return false; + } + + case Stage::WAIT_LEVEL: { + // When pitch has raised past lower limit used by speed controller, wait for airspeed to approach + // target value before handing over control of pitch demand to speed controller + bool pitchup_complete = ahrs.pitch_sensor*0.01 > MIN(0, aparm.pitch_limit_min); + const float pitch_lag_time = 1.0f * sqrtf(ahrs.get_EAS2TAS()); + float aspeed; + const float aspeed_derivative = (ahrs.get_accel().x + GRAVITY_MSS * ahrs.get_DCM_rotation_body_to_ned().c.x) / ahrs.get_EAS2TAS(); + bool airspeed_low = ahrs.airspeed_estimate(aspeed) ? (aspeed + aspeed_derivative * pitch_lag_time) < 0.01f * (float)plane.target_airspeed_cm : true; + bool roll_control_lost = fabsf(ahrs.roll_sensor*0.01) > aparm.roll_limit; + if (pitchup_complete && airspeed_low && !roll_control_lost) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup level r=%.1f p=%.1f alt %.1fm AMSL", + ahrs.roll_sensor*0.01, ahrs.pitch_sensor*0.01, current_loc.alt*0.01); + break; + } else if (pitchup_complete && roll_control_lost) { + // push nose down and wait to get roll control back + gcs().send_text(MAV_SEVERITY_ALERT, "Pullup level roll bad r=%.1f p=%.1f", + ahrs.roll_sensor*0.01, + ahrs.pitch_sensor*0.01); + stage = Stage::PUSH_NOSE_DOWN; + } + return false; + } + case Stage::NONE: + break; + } + + // all done + stage = Stage::NONE; + return true; +} + +/* + stabilize during pullup from balloon drop + */ +void GliderPullup::stabilize_pullup(void) +{ + const float speed_scaler = plane.get_speed_scaler(); + switch (stage) { + case Stage::WAIT_AIRSPEED: { + plane.pitchController.reset_I(); + plane.yawController.reset_I(); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + plane.nav_pitch_cd = 0; + plane.nav_roll_cd = 0; + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler)); + ng_demand = 0.0; + break; + } + case Stage::WAIT_PITCH: { + plane.yawController.reset_I(); + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler)); + float aspeed; + const auto &ahrs = plane.ahrs; + if (ahrs.airspeed_estimate(aspeed)) { + // apply a rate of change limit to the ng pullup demand + ng_demand += MAX(ng_jerk_limit / ahrs.get_EAS2TAS(), 0.1f) * plane.scheduler.get_loop_period_s(); + ng_demand = MIN(ng_demand, ng_limit); + const float VTAS_ref = ahrs.get_EAS2TAS() * aspeed; + const float pullup_accel = ng_demand * GRAVITY_MSS; + const float demanded_rate_dps = degrees(pullup_accel / VTAS_ref); + const uint32_t elev_trim_offset_cd = 4500.0f * elev_offset * (1.0f - ng_demand / ng_limit); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_trim_offset_cd + plane.pitchController.get_rate_out(demanded_rate_dps, speed_scaler)); + } else { + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500); + } + break; + } + case Stage::PUSH_NOSE_DOWN: { + plane.nav_pitch_cd = plane.aparm.pitch_limit_min*100; + plane.stabilize_pitch(); + plane.nav_roll_cd = 0; + plane.stabilize_roll(); + plane.stabilize_yaw(); + ng_demand = 0.0f; + break; + } + case Stage::WAIT_LEVEL: + plane.nav_pitch_cd = MAX((plane.aparm.pitch_limit_min + 5), pitch_dem)*100; + plane.stabilize_pitch(); + plane.nav_roll_cd = 0; + plane.stabilize_roll(); + plane.stabilize_yaw(); + ng_demand = 0.0f; + break; + case Stage::NONE: + break; + } + + // we have done stabilisation + plane.last_stabilize_ms = AP_HAL::millis(); +} + +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED diff --git a/ArduPlane/pullup.h b/ArduPlane/pullup.h new file mode 100644 index 00000000000000..d285b4bfc5eea7 --- /dev/null +++ b/ArduPlane/pullup.h @@ -0,0 +1,50 @@ +#pragma once + +/* + support for pullup after NAV_ALTITUDE_WAIT for gliders + */ + +#ifndef AP_PLANE_GLIDER_PULLUP_ENABLED +#define AP_PLANE_GLIDER_PULLUP_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + +class GliderPullup +{ +public: + GliderPullup(void); + + void reset(void) { + stage = Stage::NONE; + } + bool in_pullup() const; + bool verify_pullup(void); + void stabilize_pullup(void); + bool pullup_complete(void); + bool pullup_start(void); + + enum class Stage : uint8_t { + NONE=0, + WAIT_AIRSPEED, + WAIT_PITCH, + WAIT_LEVEL, + PUSH_NOSE_DOWN, + }; + + static const struct AP_Param::GroupInfo var_info[]; + +private: + Stage stage; + AP_Int8 enable; + AP_Float elev_offset; // fraction of full elevator applied during WAIT_AIRSPEED and released during WAIT_PITCH + AP_Float ng_limit; + AP_Float airspeed_start; + AP_Float pitch_start; + AP_Float ng_jerk_limit; + AP_Float pitch_dem; + float ng_demand; +}; + +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED + diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0a41b5551b6e6e..3c0d2e18fd259f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1764,7 +1764,7 @@ void QuadPlane::update(void) const bool motors_active = in_vtol_mode() || assisted_flight; if (motors_active && (motors->get_spool_state() != AP_Motors::SpoolState::SHUT_DOWN)) { // log RATE at main loop rate - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); + attitude_control->Write_Rate(*pos_control); // log CTRL and MOTB at 10 Hz if (now - last_ctrl_log_ms > 100) { diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index e601c94fde10eb..3862b390b7245a 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -8,16 +8,17 @@ */ void Plane::set_control_channels(void) { + // the library gaurantees that these are non-nullptr: if (g.rudder_only) { // in rudder only mode the roll and rudder channels are the // same. - channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_roll = &rc().get_yaw_channel(); } else { - channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); + channel_roll = &rc().get_roll_channel(); } - channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1); - channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); - channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_pitch = &rc().get_pitch_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_rudder = &rc().get_yaw_channel(); // set rc channel ranges channel_roll->set_angle(SERVO_MAX); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index cf1c6f9abcbe39..b66dc2b644a857 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -183,7 +183,7 @@ void Sub::ten_hz_logging_loop() // log attitude data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - ahrs_view.Write_Rate(motors, attitude_control, pos_control); + attitude_control.Write_Rate(pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); @@ -222,7 +222,7 @@ void Sub::twentyfive_hz_logging() { if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); - ahrs_view.Write_Rate(motors, attitude_control, pos_control); + attitude_control.Write_Rate(pos_control); if (should_log(MASK_LOG_PID)) { logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info()); diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index e197ad3ba4d9cb..88642afe26e3d2 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -59,9 +59,7 @@ void Sub::Log_Write_Control_Tuning() // Write an attitude packet void Sub::Log_Write_Attitude() { - Vector3f targets = attitude_control.get_att_target_euler_cd(); - targets.z = wrap_360_cd(targets.z); - ahrs.Write_Attitude(targets); + ahrs.Write_Attitude(attitude_control.get_att_target_euler_rad() * RAD_TO_DEG); AP::ahrs().Log_Write(); } diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index fddce3e914dd21..4b0c5a9a709e2b 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -15,10 +15,11 @@ void Blimp::default_dead_zones() void Blimp::init_rc_in() { - channel_right = rc().channel(rcmap.roll()-1); - channel_front = rc().channel(rcmap.pitch()-1); - channel_up = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + // the library gaurantees that these are non-nullptr: + channel_right = &rc().get_roll_channel(); + channel_front = &rc().get_pitch_channel(); + channel_up = &rc().get_throttle_channel(); + channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges channel_right->set_angle(RC_SCALE); diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 3490e2a703d33d..f13ad700add8d5 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -7,8 +7,8 @@ // Write an attitude packet void Rover::Log_Write_Attitude() { - float desired_pitch_cd = degrees(g2.attitude_control.get_desired_pitch()) * 100.0f; - const Vector3f targets(0.0f, desired_pitch_cd, 0.0f); + float desired_pitch = degrees(g2.attitude_control.get_desired_pitch()); + const Vector3f targets(0.0f, desired_pitch, 0.0f); ahrs.Write_Attitude(targets); diff --git a/Rover/mode.h b/Rover/mode.h index c338d8cea2b571..6b7b579702ad01 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -499,6 +499,7 @@ class ModeCircle : public Mode float angle_total_rad; // total angle in radians that vehicle has circled bool reached_edge; // true once vehicle has reached edge of circle float dist_to_edge_m; // distance to edge of circle in meters (equivalent to crosstrack error) + bool tracking_back; // true if the vehicle is trying to track back onto the circle }; class ModeGuided : public Mode diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp index 7bac1e9f50806b..c0a76862071950 100644 --- a/Rover/mode_circle.cpp +++ b/Rover/mode_circle.cpp @@ -70,7 +70,7 @@ bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir return true; } -// initialize dock mode +// initialize circle mode from current position bool ModeCircle::_enter() { // capture starting point and yaw @@ -85,9 +85,15 @@ bool ModeCircle::_enter() target.yaw_rad = AP::ahrs().get_yaw(); target.speed = 0; + // record center as location (only used for reporting) + config.center_loc = rover.current_loc; + // check speed around circle does not lead to excessive lateral acceleration check_config_speed(); + // reset tracking_back + tracking_back = false; + // calculate speed, accel and jerk limits // otherwise the vehicle uses wp_nav default speed limit float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max()); @@ -146,13 +152,20 @@ void ModeCircle::update() // Update distance to destination and distance to edge const Vector2f center_to_veh = curr_pos - config.center_pos; - _distance_to_destination = center_to_veh.length(); - dist_to_edge_m = fabsf(_distance_to_destination - config.radius); + _distance_to_destination = (target.pos.tofloat() - curr_pos).length(); + dist_to_edge_m = fabsf(center_to_veh.length() - config.radius); // Update depending on stage if (!reached_edge) { update_drive_to_radius(); - + } else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && !tracking_back) { + // if more than 2* turn_radius outside circle radius, slow vehicle by 20% + config.speed = 0.8 * config.speed; + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: cannot keep up, slowing to %.1fm/s", config.speed); + tracking_back = true; + } else if (dist_to_edge_m < MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && tracking_back) { + // if within turn_radius, call the vehicle back on track + tracking_back = false; } else { update_circling(); } @@ -190,12 +203,16 @@ void ModeCircle::update_circling() const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max); target.speed += accel_fb; - // calculate angular rate and update target angle - const float circumference = 2.0 * M_PI * config.radius; - const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0); - const float angle_dt = angular_rate_rad * rover.G_Dt; - target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt); - angle_total_rad += angle_dt; + // calculate angular rate and update target angle, if the vehicle is not trying to track back + if (!tracking_back) { + const float circumference = 2.0 * M_PI * config.radius; + const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0); + const float angle_dt = angular_rate_rad * rover.G_Dt; + target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt); + angle_total_rad += angle_dt; + } else { + init_target_yaw_rad(); + } // calculate target point's position, velocity and acceleration target.pos = config.center_pos.topostype(); @@ -221,7 +238,7 @@ float ModeCircle::wp_bearing() const return 0; } // calc vector from circle center to vehicle - Vector2f veh_to_center = (config.center_pos - curr_pos_NE); + Vector2f veh_to_center = (target.pos.tofloat() - curr_pos_NE); if (veh_to_center.is_zero()) { return 0; } @@ -266,6 +283,7 @@ bool ModeCircle::set_desired_speed(float speed_ms) bool ModeCircle::get_desired_location(Location& destination) const { destination = config.center_loc; + destination.offset_bearing(degrees(target.yaw_rad), config.radius); return true; } diff --git a/Rover/radio.cpp b/Rover/radio.cpp index cf1f159689c957..9fdb5c0c980f62 100644 --- a/Rover/radio.cpp +++ b/Rover/radio.cpp @@ -6,9 +6,10 @@ void Rover::set_control_channels(void) { // check change on RCMAP - channel_steer = rc().channel(rcmap.roll()-1); - channel_throttle = rc().channel(rcmap.throttle()-1); - channel_lateral = rc().channel(rcmap.yaw()-1); + // the library gaurantees that these are non-nullptr: + channel_steer = &rc().get_roll_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_lateral = &rc().get_yaw_channel(); // set rc channel ranges channel_steer->set_angle(SERVO_MAX); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 2908c0fddfeb02..e0196045a972f8 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -86,9 +86,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(format_version, "FORMAT_VERSION", 0), // @Param: CAN_NODE - // @DisplayName: UAVCAN node that is used for this network - // @Description: UAVCAN node should be set implicitly or 0 for dynamic node allocation - // @Range: 0 250 + // @DisplayName: DroneCAN node ID used by this node on all networks + // @Description: Value of 0 requests any ID from a DNA server, any other value sets that ID ignoring DNA + // @Range: 0 127 // @User: Advanced // @RebootRequired: True GSCALAR(can_node, "CAN_NODE", HAL_CAN_DEFAULT_NODE_ID), diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 036cbed6dffe33..0a184888cbae10 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -125,10 +125,7 @@ HAL_GPIO_PIN_TERMCAN1 }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(HAL_GPIO_PIN_TERMCAN1) -#ifndef HAL_CAN_DEFAULT_NODE_ID -#define HAL_CAN_DEFAULT_NODE_ID CANARD_BROADCAST_NODE_ID -#endif -uint8_t PreferredNodeID = HAL_CAN_DEFAULT_NODE_ID; +uint8_t user_set_node_id = HAL_CAN_DEFAULT_NODE_ID; #ifndef AP_PERIPH_PROBE_CONTINUOUS #define AP_PERIPH_PROBE_CONTINUOUS 0 @@ -472,7 +469,7 @@ void AP_Periph_FW::handle_allocation_response(CanardInstance* canard_instance, C dronecan.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; printf("Matching allocation response: %d\n", msg.unique_id.len); - } else { + } else if (msg.node_id != CANARD_BROADCAST_NODE_ID) { // new ID valid? (if not we will time out and start over) // Allocation complete - copying the allocated node ID from the message canardSetLocalNodeID(canard_instance, msg.node_id); printf("IF%d Node ID allocated: %d\n", dronecan.dna_interface, msg.node_id); @@ -1550,7 +1547,7 @@ bool AP_Periph_FW::can_do_dna() // Structure of the request is documented in the DSDL definition // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; - allocation_request[0] = (uint8_t)(PreferredNodeID << 1U); + allocation_request[0] = 0; // we are only called if the user has not set an ID, so request any ID if (dronecan.node_id_allocation_unique_id_offset == 0) { allocation_request[0] |= 1; // First part of unique ID @@ -1590,7 +1587,7 @@ void AP_Periph_FW::can_start() node_status.uptime_sec = AP_HAL::millis() / 1000U; if (g.can_node >= 0 && g.can_node < 128) { - PreferredNodeID = g.can_node; + user_set_node_id = g.can_node; } #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) @@ -1662,8 +1659,8 @@ void AP_Periph_FW::can_start() canardInit(&dronecan.canard, (uint8_t *)dronecan.canard_memory_pool, sizeof(dronecan.canard_memory_pool), onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, this); - if (PreferredNodeID != CANARD_BROADCAST_NODE_ID) { - canardSetLocalNodeID(&dronecan.canard, PreferredNodeID); + if (user_set_node_id != CANARD_BROADCAST_NODE_ID) { + canardSetLocalNodeID(&dronecan.canard, user_set_node_id); } } diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 2bc27fc9bb0a26..334ca0d6a0e77a 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -16,6 +16,7 @@ # modify our search path: sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../libraries/AP_HAL_ChibiOS/hwdef/scripts')) import chibios_hwdef +import build_options class BoardMeta(type): def __init__(cls, name, bases, dct): @@ -160,8 +161,16 @@ def srcpath(path): ) cfg.msg("Enabled custom controller", 'no', color='YELLOW') - if cfg.options.enable_ppp: - env.CXXFLAGS += ['-DAP_NETWORKING_BACKEND_PPP=1'] + # support enabling any option in build_options.py + for opt in build_options.BUILD_OPTIONS: + enable_option = opt.config_option().replace("-","_") + disable_option = "disable_" + enable_option[len("enable-"):] + if getattr(cfg.options, enable_option, False): + env.CXXFLAGS += ['-D%s=1' % opt.define] + cfg.msg("Enabled %s" % opt.label, 'yes', color='GREEN') + elif getattr(cfg.options, disable_option, False): + env.CXXFLAGS += ['-D%s=0' % opt.define] + cfg.msg("Enabled %s" % opt.label, 'no', color='YELLOW') if cfg.options.disable_networking: env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=0'] @@ -488,12 +497,6 @@ def configure_env(self, cfg, env): # We always want to use PRI format macros cfg.define('__STDC_FORMAT_MACROS', 1) - if cfg.options.enable_ekf2: - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] - - if cfg.options.disable_ekf3: - env.CXXFLAGS += ['-DHAL_NAVEKF3_AVAILABLE=0'] - if cfg.options.postype_single: env.CXXFLAGS += ['-DHAL_WITH_POSTYPE_DOUBLE=0'] diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 60fd1ef4ee2418..785bce15bcee55 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -273,12 +273,14 @@ def run(self): else: descriptor = b'\x40\xa2\xe4\xf1\x64\x68\x91\x06' - img = open(self.inputs[0].abspath(), 'rb').read() + elf_file = self.inputs[0].abspath() + bin_file = self.inputs[1].abspath() + img = open(bin_file, 'rb').read() offset = img.find(descriptor) if offset == -1: Logs.info("No APP_DESCRIPTOR found") return - offset += 8 + offset += len(descriptor) # next 8 bytes is 64 bit CRC. We set first 4 bytes to # CRC32 of image before descriptor and 2nd 4 bytes # to CRC32 of image after descriptor. This is very efficient @@ -310,7 +312,19 @@ def run(self): desc = struct.pack('/dev/null| grep venv) if [[ $VENV_LOOKUP ]]; then $APT_GET install $PYTHON_VENV_PACKAGE - python3 -m venv $HOME/venv-ardupilot + python3 -m venv --system-site-packages $HOME/venv-ardupilot # activate it: SOURCE_LINE="source $HOME/venv-ardupilot/bin/activate" diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index a4e660027a8ed6..cc67f661c8d23b 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -92,6 +92,11 @@ colcon test --packages-select ardupilot_dds_tests colcon test-result --all --verbose ``` +To debug a specific test, you can do the following: +``` +colcon --log-level DEBUG test --packages-select ardupilot_dds_tests --event-handlers=console_direct+ --pytest-args -k test_dds_udp_geopose_msg_recv -s +``` + ## Install macOS The install procedure on macOS is similar, except that all dependencies diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py index 5f00a3fb93d561..27de6f272f2ed8 100755 --- a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py +++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py @@ -144,7 +144,7 @@ def achieved_goal(goal_global_pos, cur_geopose): p2 = (cur_pos.latitude, cur_pos.longitude, cur_pos.altitude) flat_distance = distance.distance(p1[:2], p2[:2]).m - euclidian_distance = math.sqrt(flat_distance ** 2 + (p2[2] - p1[2]) ** 2) + euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2) print(f"Goal is {euclidian_distance} meters away") return euclidian_distance < 150 diff --git a/Tools/ros2/ardupilot_dds_tests/package.xml b/Tools/ros2/ardupilot_dds_tests/package.xml index 528f4794a1052e..c3eb345058959e 100644 --- a/Tools/ros2/ardupilot_dds_tests/package.xml +++ b/Tools/ros2/ardupilot_dds_tests/package.xml @@ -27,15 +27,17 @@ ament_lint_auto ardupilot_msgs ardupilot_sitl - builtin_interfaces + builtin_interfaces + geographic_msgs launch launch_pytest + micro_ros_agent launch_ros micro_ros_msgs python3-pytest rclpy sensor_msgs - + python3-scipy ament_python diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py new file mode 100644 index 00000000000000..64bb0329bd77c1 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_geopose_msg_received.py @@ -0,0 +1,186 @@ +# 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 . + +"""Bring up ArduPilot SITL and check the GeoPose message is being published.""" + +import launch_pytest +import math +import pytest +import rclpy +import rclpy.node +from scipy.spatial.transform import Rotation as R +import threading + +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 geographic_msgs.msg import GeoPoseStamped + +TOPIC = "ap/geopose/filtered" +# Copied from locations.txt +CMAC_LAT = -35.363261 +CMAC_LON = 149.165230 +CMAC_ABS_ALT = 584 +CMAC_HEADING = 353 + + +def ros_quat_to_heading(quat): + # By default, scipy follows scalar-last order – (x, y, z, w) + rot = R.from_quat([quat.x, quat.y, quat.z, quat.w]) + r, p, y = rot.as_euler(seq="xyz", degrees=True) + return y + + +def validate_position_cmac(position): + """Return true if the vehicle is at CMAC.""" + LAT_LON_TOL = 1e-5 + return ( + math.isclose(position.latitude, CMAC_LAT, abs_tol=LAT_LON_TOL) + and math.isclose(position.longitude, CMAC_LON, abs_tol=LAT_LON_TOL) + and math.isclose(position.altitude, CMAC_ABS_ALT, abs_tol=1.0) + ) + + +def validate_heading_cmac(orientation): + """Return true if the vehicle is facing the right way for CMAC.""" + return math.isclose(ros_quat_to_heading(orientation), CMAC_HEADING) + + +class GeoPoseListener(rclpy.node.Node): + """Subscribe to GeoPoseStamped messages.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("geopose_listener") + self.msg_event_object = threading.Event() + self.position_correct_event_object = threading.Event() + + # Declare and acquire `topic` parameter + self.declare_parameter("topic", TOPIC) + self.topic = self.get_parameter("topic").get_parameter_value().string_value + + def start_subscriber(self): + """Start the subscriber.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.subscription = self.create_subscription(GeoPoseStamped, self.topic, self.subscriber_callback, qos_profile) + + # Add a spin thread. + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def subscriber_callback(self, msg): + """Process a GeoPoseStamped message.""" + self.msg_event_object.set() + + position = msg.pose.position + + self.get_logger().info("From AP : Position [lat:{}, lon: {}]".format(position.latitude, position.longitude)) + + if validate_position_cmac(msg.pose.position): + self.position_correct_event_object.set() + + +@launch_pytest.fixture +def launch_sitl_copter_dds_serial(sitl_copter_dds_serial): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_serial + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@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_serial) +def test_dds_serial_geopose_msg_recv(launch_context, launch_sitl_copter_dds_serial): + """Test position messages are published by AP_DDS.""" + _, actions = launch_sitl_copter_dds_serial + virtual_ports = actions["virtual_ports"].action + 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, virtual_ports, timeout=2) + 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 = GeoPoseListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + pose_correct_flag = node.position_correct_event_object.wait(timeout=10.0) + assert pose_correct_flag, f"Did not receive correct position." + finally: + rclpy.shutdown() + yield + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_geopose_msg_recv(launch_context, launch_sitl_copter_dds_udp): + """Test position messages are published 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 = GeoPoseListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + pose_correct_flag = node.position_correct_event_object.wait(timeout=10.0) + assert pose_correct_flag, f"Did not receive correct position." + finally: + rclpy.shutdown() + yield diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py index 05ff1343d271df..835b3e95d30e00 100644 --- a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_navsat_msg_received.py @@ -30,9 +30,11 @@ from sensor_msgs.msg import NavSatFix +TOPIC = "ap/navsat/navsat0" + class NavSatFixListener(rclpy.node.Node): - """Subscribe to NavSatFix messages on /ap/navsat/navsat0.""" + """Subscribe to NavSatFix messages.""" def __init__(self): """Initialise the node.""" @@ -40,7 +42,7 @@ def __init__(self): self.msg_event_object = threading.Event() # Declare and acquire `topic` parameter - self.declare_parameter("topic", "ap/navsat/navsat0") + self.declare_parameter("topic", TOPIC) self.topic = self.get_parameter("topic").get_parameter_value().string_value def start_subscriber(self): @@ -117,7 +119,7 @@ def test_dds_serial_navsat_msg_recv(launch_context, launch_sitl_copter_dds_seria node = NavSatFixListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ap/navsat/navsat0' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield @@ -141,7 +143,7 @@ def test_dds_udp_navsat_msg_recv(launch_context, launch_sitl_copter_dds_udp): node = NavSatFixListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ap/navsat/navsat0' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py index 5ffa1de6e0b11c..dc33b88c0e9ec3 100644 --- a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_time_msg_received.py @@ -26,9 +26,11 @@ from builtin_interfaces.msg import Time +TOPIC = "ap/time" + class TimeListener(rclpy.node.Node): - """Subscribe to Time messages on /ap/time.""" + """Subscribe to Time messages.""" def __init__(self): """Initialise the node.""" @@ -36,7 +38,7 @@ def __init__(self): self.msg_event_object = threading.Event() # Declare and acquire `topic` parameter. - self.declare_parameter("topic", "ap/time") + self.declare_parameter("topic", TOPIC) self.topic = self.get_parameter("topic").get_parameter_value().string_value def start_subscriber(self): @@ -89,7 +91,7 @@ def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): @pytest.mark.launch(fixture=launch_sitl_copter_dds_serial) def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial): - """Test /ap/time is published by AP_DDS.""" + """Test Time is published by AP_DDS.""" _, actions = launch_sitl_copter_dds_serial virtual_ports = actions["virtual_ports"].action micro_ros_agent = actions["micro_ros_agent"].action @@ -107,7 +109,7 @@ def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial) node = TimeListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ROS_Time' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield @@ -115,7 +117,7 @@ def test_dds_serial_time_msg_recv(launch_context, launch_sitl_copter_dds_serial) @pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) def test_dds_udp_time_msg_recv(launch_context, launch_sitl_copter_dds_udp): - """Test /ap/time is published by AP_DDS.""" + """Test Time is published by AP_DDS.""" _, actions = launch_sitl_copter_dds_udp micro_ros_agent = actions["micro_ros_agent"].action mavproxy = actions["mavproxy"].action @@ -131,7 +133,7 @@ def test_dds_udp_time_msg_recv(launch_context, launch_sitl_copter_dds_udp): node = TimeListener() node.start_subscriber() msgs_received_flag = node.msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive 'ROS_Time' msgs." + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." finally: rclpy.shutdown() yield diff --git a/Tools/ros2/ardupilot_sitl/CMakeLists.txt b/Tools/ros2/ardupilot_sitl/CMakeLists.txt index 1be507e13fef67..d1d2017463512e 100644 --- a/Tools/ros2/ardupilot_sitl/CMakeLists.txt +++ b/Tools/ros2/ardupilot_sitl/CMakeLists.txt @@ -34,7 +34,7 @@ set(WAF_DISABLE_SCRIPTING $<$:"--disable-sc set(WAF_DISABLE_WATCHDOG $<$:"--disable-watchdog">) set(WAF_ENABLE_DDS $<$:"--enable-dds">) set(WAF_ENABLE_NETWORKING_TESTS $<$:"--enable-networking-tests">) -set(WAF_ENABLE_PPP $<$:"--enable-ppp">) +set(WAF_ENABLE_PPP $<$:"--enable-PPP">) add_custom_target(ardupilot_configure ALL ${WAF_COMMAND} configure --board sitl diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index 1513e9cbe3f8bc..ae1360917324da 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -321,7 +321,7 @@ for t in $CI_BUILD_TARGET; do if [ "$t" == "CubeOrange-PPP" ]; then echo "Building CubeOrange-PPP" - $waf configure --board CubeOrange --enable-ppp + $waf configure --board CubeOrange --enable-PPP $waf clean $waf copter continue @@ -329,7 +329,7 @@ for t in $CI_BUILD_TARGET; do if [ "$t" == "CubeOrange-EKF2" ]; then echo "Building CubeOrange with EKF2 enabled" - $waf configure --board CubeOrange --enable-ekf2 + $waf configure --board CubeOrange --enable-EKF2 $waf clean $waf copter continue diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 65f111c8a4e7af..078e2c1d1f921b 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -23,6 +23,10 @@ def __init__(self, self.default = default self.dependency = dependency + def config_option(self): + '''the name of the configure option to be used by waf''' + return "enable-" + self.label.replace(" ", "-") + # list of build options to offer NOTE: the dependencies must be # written as a single string with commas and no spaces, @@ -136,6 +140,7 @@ def __init__(self, Feature('Camera', 'Camera_Servo', 'AP_CAMERA_SERVO_ENABLED', 'Enable Camera Trigger via Servo support', 0, 'Camera'), Feature('Camera', 'Camera_Solo', 'AP_CAMERA_SOLOGIMBAL_ENABLED', 'Enable Solo Gimbal support', 0, 'Camera'), Feature('Camera', 'Camera_FOV_Status', 'AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'Enable Camera FOV Status send to GCS', 0, 'Camera,MOUNT'), # noqa: E501 + Feature('Camera', 'Camera_ThermalRange', 'AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'Enable Camera Thermal Range send to GCS', 0, 'Camera,MOUNT'), # noqa: E501 Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None), @@ -212,6 +217,7 @@ def __init__(self, Feature('Plane', 'PLANE_BLACKBOX', 'AP_PLANE_BLACKBOX_LOGGING', 'Enable blackbox logging', 0, None), Feature('Plane', 'AP_TX_TUNING', 'AP_TUNING_ENABLED', 'Enable TX-based tuning parameter adjustments', 0, None), Feature('Plane', 'PLANE_GUIDED_SLEW', 'AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', 'Enable offboard-guided slew commands', 0, None), # noqa:401 + Feature('Plane', 'PLANE_GLIDER_PULLUP', 'AP_PLANE_GLIDER_PULLUP_ENABLED', 'Enable glider pullup support', 0, None), Feature('RC', 'RC_Protocol', 'AP_RCPROTOCOL_ENABLED', "Enable Serial RC Protocol support", 0, None), # NOQA: E501 Feature('RC', 'RC_CRSF', 'AP_RCPROTOCOL_CRSF_ENABLED', "Enable CRSF RC Protocol", 0, "RC_Protocol"), # NOQA: E501 @@ -382,14 +388,14 @@ def __init__(self, Feature('Airspeed Drivers', 'Analog', 'AP_AIRSPEED_ANALOG_ENABLED', 'Enable Analog Airspeed', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'ASP5033', 'AP_AIRSPEED_ASP5033_ENABLED', 'ENABLE ASP5033 AIRSPEED', 0, 'AIRSPEED'), # NOQA: E501 - Feature('Airspeed Drivers', 'DLVR', 'AP_AIRSPEED_DLVR_ENABLED', 'ENABLE DLVR AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'MS4525', 'AP_AIRSPEED_MS4525_ENABLED', 'ENABLE MS4525 AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'MS5525', 'AP_AIRSPEED_MS5525_ENABLED', 'ENABLE MS5525 AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'MSP_AIRSPEED', 'AP_AIRSPEED_MSP_ENABLED', 'ENABLE MSP AIRSPEED', 0, 'AIRSPEED,MSP,OSD'), - Feature('Airspeed Drivers', 'NMEA_AIRSPEED', 'AP_AIRSPEED_NMEA_ENABLED', 'ENABLE NMEA AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'ENABLE SDP3X AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'ENABLE DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501 + Feature('Airspeed Drivers', 'ASP5033', 'AP_AIRSPEED_ASP5033_ENABLED', 'Enable ASP5033 AIRSPEED', 0, 'AIRSPEED'), # NOQA: E501 + Feature('Airspeed Drivers', 'DLVR', 'AP_AIRSPEED_DLVR_ENABLED', 'Enable DLVR AIRSPEED', 0, 'AIRSPEED'), + Feature('Airspeed Drivers', 'MS4525', 'AP_AIRSPEED_MS4525_ENABLED', 'Enable MS4525 AIRSPEED', 0, 'AIRSPEED'), + Feature('Airspeed Drivers', 'MS5525', 'AP_AIRSPEED_MS5525_ENABLED', 'Enable MS5525 AIRSPEED', 0, 'AIRSPEED'), + Feature('Airspeed Drivers', 'MSP_AIRSPEED', 'AP_AIRSPEED_MSP_ENABLED', 'Enable MSP AIRSPEED', 0, 'AIRSPEED,MSP,OSD'), + Feature('Airspeed Drivers', 'NMEA_AIRSPEED', 'AP_AIRSPEED_NMEA_ENABLED', 'Enable NMEA AIRSPEED', 0, 'AIRSPEED'), + Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'), + Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501 Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None), Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz"), # noqa: E501 @@ -413,7 +419,7 @@ def __init__(self, Feature('Filesystem', 'FILESYSTEM_SYS', 'AP_FILESYSTEM_SYS_ENABLED', 'Enable @SYS/ filesystem', 0, None), Feature('Filesystem', 'APJ_TOOL_PARAMETERS', 'FORCE_APJ_DEFAULT_PARAMETERS', 'Enable apj_tool parameter area', 0, None), - Feature('Networking', 'PPP Support', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None), + Feature('Networking', 'PPP', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None), Feature('DroneCAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None), ] diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index d14cadcf173937..54c13e39c92c45 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -136,6 +136,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_CAMERA_ENABLED', 'AP_Camera::var_info',), ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P.*)::trigger_pic',), ('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'), + ('AP_CAMERA_SEND_THERMAL_RANGE_ENABLED', 'AP_Camera::send_camera_thermal_range'), ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',), ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',), @@ -170,6 +171,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_CHECK_FIRMWARE_ENABLED', 'AP_CheckFirmware::check_signed_bootloader',), ('HAL_QUADPLANE_ENABLED', 'QuadPlane::QuadPlane',), + ('AP_PLANE_GLIDER_PULLUP_ENABLED', 'GliderPullup::in_pullup',), ('QAUTOTUNE_ENABLED', 'ModeQAutotune::_enter',), ('HAL_SOARING_ENABLED', 'SoaringController::var_info',), ('HAL_LANDING_DEEPSTALL_ENABLED', r'AP_Landing_Deepstall::override_servos',), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 14d3a1fc7f32d3..d95651fbd03e82 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1,7 +1,9 @@ #include "AC_AttitudeControl.h" +#include "AC_PosControl.h" #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -1240,3 +1242,51 @@ void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, fl pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate; yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate; } + +#if HAL_LOGGING_ENABLED +// Write a rate packet +void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const +{ + const Vector3f &rate_targets = rate_bf_targets(); + const Vector3f &accel_target = pos_control.get_accel_target_cmss(); + const Vector3f &gyro_rate = _rate_gyro; + const struct log_Rate pkt_rate{ + LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), + time_us : _rate_gyro_time_us, + control_roll : degrees(rate_targets.x), + roll : degrees(gyro_rate.x), + roll_out : _motors.get_roll()+_motors.get_roll_ff(), + control_pitch : degrees(rate_targets.y), + pitch : degrees(gyro_rate.y), + pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(), + control_yaw : degrees(rate_targets.z), + yaw : degrees(gyro_rate.z), + yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), + control_accel : (float)accel_target.z, + accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f), + accel_out : _motors.get_throttle(), + throttle_slew : _motors.get_throttle_slew_rate() + }; + AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate)); + + /* + log P/PD gain scale if not == 1.0 + */ + const Vector3f &scale = get_last_angle_P_scale(); + const Vector3f &pd_scale = _pd_scale_used; + if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) { + const struct log_ATSC pkt_ATSC { + LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG), + time_us : _rate_gyro_time_us, + scaleP_x : scale.x, + scaleP_y : scale.y, + scaleP_z : scale.z, + scalePD_x : pd_scale.x, + scalePD_y : pd_scale.y, + scalePD_z : pd_scale.z, + }; + AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC)); + } +} + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index b3968282580d51..104720f38a56f1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -428,8 +428,8 @@ class AC_AttitudeControl { // purposes void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; } - // get the value of the PD scale that was used in the last loop, for logging - const Vector3f &get_PD_scale_logging(void) const { return _pd_scale_used; } + // write RATE message + void Write_Rate(const AC_PosControl &pos_control) const; // User settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -487,6 +487,11 @@ class AC_AttitudeControl { AP_Float _land_pitch_mult; AP_Float _land_yaw_mult; + // latest gyro value use by the rate_controller + Vector3f _rate_gyro; + // timestamp of the latest gyro value used by the rate controller + uint64_t _rate_gyro_time_us; + // Intersampling period in seconds float _dt; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 39c9ad277e9828..4c67892fe0f170 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -422,7 +422,8 @@ void AC_AttitudeControl_Heli::rate_controller_run() { _ang_vel_body += _sysid_ang_vel_body; - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); // call rate controllers and send output to motors object // if using a flybar passthrough roll and pitch directly to motors @@ -430,12 +431,12 @@ void AC_AttitudeControl_Heli::rate_controller_run() _motors.set_roll(_passthrough_roll / 4500.0f); _motors.set_pitch(_passthrough_pitch / 4500.0f); } else { - rate_bf_to_motor_roll_pitch(gyro_latest, _ang_vel_body.x, _ang_vel_body.y); + rate_bf_to_motor_roll_pitch(_rate_gyro, _ang_vel_body.x, _ang_vel_body.y); } if (_flags_heli.tail_passthrough) { _motors.set_yaw(_passthrough_yaw / 4500.0f); } else { - _motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _ang_vel_body.z)); + _motors.set_yaw(rate_target_to_motor_yaw(_rate_gyro.z, _ang_vel_body.z)); } _sysid_ang_vel_body.zero(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 947975a0a16868..91378c8bfa0677 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -449,15 +449,16 @@ void AC_AttitudeControl_Multi::rate_controller_run() _ang_vel_body += _sysid_ang_vel_body; - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); _sysid_ang_vel_body.zero(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index 7f2791c241a36f..e83abb39aeb876 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -423,10 +423,12 @@ void AC_AttitudeControl_Sub::rate_controller_run() // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); - Vector3f gyro_latest = _ahrs.get_gyro_latest(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll)); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch)); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw)); + _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro_time_us = AP_HAL::micros64(); + + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll)); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch)); + _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw)); control_monitor_update(); } diff --git a/libraries/AC_AttitudeControl/LogStructure.h b/libraries/AC_AttitudeControl/LogStructure.h index 9b5fe8df95bb3c..8290ec3d09f202 100644 --- a/libraries/AC_AttitudeControl/LogStructure.h +++ b/libraries/AC_AttitudeControl/LogStructure.h @@ -58,6 +58,40 @@ struct PACKED log_PSCx { float accel; }; +// @LoggerMessage: RATE +// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes. +// @Field: TimeUS: Time since system startup +// @Field: RDes: vehicle desired roll rate +// @Field: R: achieved vehicle roll rate +// @Field: ROut: normalized output for Roll +// @Field: PDes: vehicle desired pitch rate +// @Field: P: vehicle pitch rate +// @Field: POut: normalized output for Pitch +// @Field: Y: achieved vehicle yaw rate +// @Field: YOut: normalized output for Yaw +// @Field: YDes: vehicle desired yaw rate +// @Field: ADes: desired vehicle vertical acceleration +// @Field: A: achieved vehicle vertical acceleration +// @Field: AOut: percentage of vertical thrust output current being used +// @Field: AOutSlew: vertical thrust output slew rate +struct PACKED log_Rate { + LOG_PACKET_HEADER; + uint64_t time_us; + float control_roll; + float roll; + float roll_out; + float control_pitch; + float pitch; + float pitch_out; + float control_yaw; + float yaw; + float yaw_out; + float control_accel; + float accel; + float accel_out; + float throttle_slew; +}; + #define PSCx_FMT "Qffffffff" #define PSCx_UNITS "smmnnnooo" #define PSCx_MULTS "F00000000" @@ -68,4 +102,6 @@ struct PACKED log_PSCx { { LOG_PSCE_MSG, sizeof(log_PSCx), \ "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \ { LOG_PSCD_MSG, sizeof(log_PSCx), \ - "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS } + "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \ + { LOG_RATE_MSG, sizeof(log_Rate), \ + "RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true } diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 64dafeab8e671d..213ae21551507a 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -443,7 +443,7 @@ void AC_AutoTune::control_attitude() #if HAL_LOGGING_ENABLED // log this iterations lean angle and rotation rate Log_AutoTuneDetails(); - ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); + attitude_control->Write_Rate(*pos_control); log_pids(); #endif diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 634fc8358e02f3..efc9675aa02b15 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -617,6 +617,22 @@ void AP_AHRS::update_EKF2(void) EKF2.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); } + + /* + if we now have an origin then set in all backends + */ + if (!done_common_origin) { + Location new_origin; + if (EKF2.getOriginLLH(new_origin)) { + done_common_origin = true; +#if HAL_NAVEKF3_AVAILABLE + EKF3.setOriginLLH(new_origin); +#endif +#if AP_AHRS_EXTERNAL_ENABLED + external.set_origin(new_origin); +#endif + } + } } } #endif @@ -686,6 +702,21 @@ void AP_AHRS::update_EKF3(void) EKF3.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); } + /* + if we now have an origin then set in all backends + */ + if (!done_common_origin) { + Location new_origin; + if (EKF3.getOriginLLH(new_origin)) { + done_common_origin = true; +#if HAL_NAVEKF2_AVAILABLE + EKF2.setOriginLLH(new_origin); +#endif +#if AP_AHRS_EXTERNAL_ENABLED + external.set_origin(new_origin); +#endif + } + } } } #endif @@ -699,6 +730,22 @@ void AP_AHRS::update_external(void) if (_active_EKF_type() == EKFType::EXTERNAL) { copy_estimates_from_backend_estimates(external_estimates); } + + /* + if we now have an origin then set in all backends + */ + if (!done_common_origin) { + Location new_origin; + if (external.get_origin(new_origin)) { + done_common_origin = true; +#if HAL_NAVEKF2_AVAILABLE + EKF2.setOriginLLH(new_origin); +#endif +#if HAL_NAVEKF3_AVAILABLE + EKF3.setOriginLLH(new_origin); +#endif + } + } } #endif // AP_AHRS_EXTERNAL_ENABLED @@ -1412,6 +1459,9 @@ bool AP_AHRS::set_origin(const Location &loc) #if HAL_NAVEKF3_AVAILABLE const bool ret3 = EKF3.setOriginLLH(loc); #endif +#if AP_AHRS_EXTERNAL_ENABLED + const bool ret_ext = external.set_origin(loc); +#endif // return success if active EKF's origin was set bool success = false; @@ -1441,7 +1491,7 @@ bool AP_AHRS::set_origin(const Location &loc) #endif #if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: - // don't allow origin set with external AHRS + success = ret_ext; break; #endif } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 98cd09fefceba6..e6aa24064a0977 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -1024,6 +1024,9 @@ class AP_AHRS { bool option_set(Options option) const { return (_options & uint16_t(option)) != 0; } + + // true when we have completed the common origin setup + bool done_common_origin; }; namespace AP { diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 46fd26f49fe951..1530a19b3d367c 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -48,18 +48,18 @@ void AP_AHRS::Write_AOA_SSA(void) const AP::logger().WriteBlock(&aoa_ssa, sizeof(aoa_ssa)); } -// Write an attitude packet +// Write an attitude packet, targets in degrees void AP_AHRS::Write_Attitude(const Vector3f &targets) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), time_us : AP_HAL::micros64(), - control_roll : (int16_t)targets.x, - roll : (int16_t)roll_sensor, - control_pitch : (int16_t)targets.y, - pitch : (int16_t)pitch_sensor, - control_yaw : (uint16_t)wrap_360_cd(targets.z), - yaw : (uint16_t)wrap_360_cd(yaw_sensor), + control_roll : targets.x, + roll : degrees(roll), + control_pitch : targets.y, + pitch : degrees(pitch), + control_yaw : wrap_360(targets.z), + yaw : wrap_360(degrees(yaw)), active : uint8_t(active_EKF_type()), }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); @@ -122,67 +122,21 @@ void AP_AHRS::write_video_stabilisation() const AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -// Write an attitude view packet +// Write an attitude view packet, targets in degrees void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), time_us : AP_HAL::micros64(), - control_roll : (int16_t)targets.x, - roll : (int16_t)roll_sensor, - control_pitch : (int16_t)targets.y, - pitch : (int16_t)pitch_sensor, - control_yaw : (uint16_t)wrap_360_cd(targets.z), - yaw : (uint16_t)wrap_360_cd(yaw_sensor), + control_roll : targets.x, + roll : degrees(roll), + control_pitch : targets.y, + pitch : degrees(pitch), + control_yaw : wrap_360(targets.z), + yaw : wrap_360(degrees(yaw)), active : uint8_t(AP::ahrs().active_EKF_type()), }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -// Write a rate packet -void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control) const -{ - const Vector3f &rate_targets = attitude_control.rate_bf_targets(); - const Vector3f &accel_target = pos_control.get_accel_target_cmss(); - const auto timeus = AP_HAL::micros64(); - const struct log_Rate pkt_rate{ - LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), - time_us : timeus, - control_roll : degrees(rate_targets.x), - roll : degrees(get_gyro().x), - roll_out : motors.get_roll()+motors.get_roll_ff(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(get_gyro().y), - pitch_out : motors.get_pitch()+motors.get_pitch_ff(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(get_gyro().z), - yaw_out : motors.get_yaw()+motors.get_yaw_ff(), - control_accel : (float)accel_target.z, - accel : (float)(-(get_accel_ef().z + GRAVITY_MSS) * 100.0f), - accel_out : motors.get_throttle(), - throttle_slew : motors.get_throttle_slew_rate() - }; - AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate)); - - /* - log P/PD gain scale if not == 1.0 - */ - const Vector3f &scale = attitude_control.get_last_angle_P_scale(); - const Vector3f &pd_scale = attitude_control.get_PD_scale_logging(); - if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) { - const struct log_ATSC pkt_ATSC { - LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG), - time_us : timeus, - scaleP_x : scale.x, - scaleP_y : scale.y, - scaleP_z : scale.z, - scalePD_x : pd_scale.x, - scalePD_y : pd_scale.y, - scalePD_z : pd_scale.z, - }; - AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC)); - } -} - #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index ff0f2293c16510..1090e112f89beb 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -174,9 +174,6 @@ class AP_AHRS_View // Logging Functions void Write_AttitudeView(const Vector3f &targets) const; - void Write_Rate(const class AP_Motors &motors, - const class AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control) const; float roll; float pitch; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 9769eee06bab94..af0c4c36d1fa4e 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -61,12 +61,12 @@ struct PACKED log_AOA_SSA { struct PACKED log_Attitude { LOG_PACKET_HEADER; uint64_t time_us; - int16_t control_roll; - int16_t roll; - int16_t control_pitch; - int16_t pitch; - uint16_t control_yaw; - uint16_t yaw; + float control_roll; + float roll; + float control_pitch; + float pitch; + float control_yaw; + float yaw; uint8_t active; }; @@ -105,40 +105,6 @@ struct PACKED log_POS { float rel_origin_alt; }; -// @LoggerMessage: RATE -// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes. -// @Field: TimeUS: Time since system startup -// @Field: RDes: vehicle desired roll rate -// @Field: R: achieved vehicle roll rate -// @Field: ROut: normalized output for Roll -// @Field: PDes: vehicle desired pitch rate -// @Field: P: vehicle pitch rate -// @Field: POut: normalized output for Pitch -// @Field: Y: achieved vehicle yaw rate -// @Field: YOut: normalized output for Yaw -// @Field: YDes: vehicle desired yaw rate -// @Field: ADes: desired vehicle vertical acceleration -// @Field: A: achieved vehicle vertical acceleration -// @Field: AOut: percentage of vertical thrust output current being used -// @Field: AOutSlew: vertical thrust output slew rate -struct PACKED log_Rate { - LOG_PACKET_HEADER; - uint64_t time_us; - float control_roll; - float roll; - float roll_out; - float control_pitch; - float pitch; - float pitch_out; - float control_yaw; - float yaw; - float yaw_out; - float control_accel; - float accel; - float accel_out; - float throttle_slew; -}; - // @LoggerMessage: VSTB // @Description: Log message for video stabilisation software such as Gyroflow // @Field: TimeUS: Time since system startup @@ -195,13 +161,11 @@ struct PACKED log_ATSC { { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \ "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \ { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ - "ATT", "QccccCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "FBBBBBB-" , true }, \ + "ATT", "QffffffB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "F000000-" , true }, \ { LOG_ORGN_MSG, sizeof(log_ORGN), \ "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ { LOG_POS_MSG, sizeof(log_POS), \ "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \ - { LOG_RATE_MSG, sizeof(log_Rate), \ - "RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \ { LOG_ATSC_MSG, sizeof(log_ATSC), \ "ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \ { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index b61bec4c9292ab..397c5d1d6e91bc 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -876,7 +876,10 @@ bool AP_Airspeed::enabled(uint8_t i) const { // return health status of sensor bool AP_Airspeed::healthy(uint8_t i) const { - bool ok = state[i].healthy && enabled(i) && sensor[i] != nullptr; + if (!enabled(i)) { + return false; + } + bool ok = state[i].healthy && sensor[i] != nullptr; #ifndef HAL_BUILD_AP_PERIPH // sanity check the offset parameter. Zero is permitted if we are skipping calibration. ok &= (fabsf(param[i].offset) > 0 || state[i].use_zero_offset || param[i].skip_cal); diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 19bfdf336c8b88..6ca52a2f0286e3 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -23,6 +23,7 @@ extern const AP_HAL::HAL &hal; #define SPL06_CHIP_ID 0x10 +#define SPA06_CHIP_ID 0x11 #define SPL06_REG_PRESSURE_B2 0x00 // Pressure MSB Register #define SPL06_REG_PRESSURE_B1 0x01 // Pressure middle byte Register @@ -44,15 +45,19 @@ extern const AP_HAL::HAL &hal; #define SPL06_REG_CHIP_ID 0x0D // Chip ID Register #define SPL06_REG_CALIB_COEFFS_START 0x10 #define SPL06_REG_CALIB_COEFFS_END 0x21 +#define SPA06_REG_CALIB_COEFFS_END 0x24 -#define SPL06_CALIB_COEFFS_LEN (SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1) +// PRESSURE_CFG_REG +#define SPL06_PRES_RATE_32HZ (0x05 << 4) // TEMPERATURE_CFG_REG #define SPL06_TEMP_USE_EXT_SENSOR (1<<7) +#define SPL06_TEMP_RATE_32HZ (0x05 << 4) // MODE_AND_STATUS_REG #define SPL06_MEAS_PRESSURE (1<<0) // measure pressure #define SPL06_MEAS_TEMPERATURE (1<<1) // measure temperature +#define SPL06_MEAS_CON_PRE_TEM 0x07 #define SPL06_MEAS_CFG_CONTINUOUS (1<<2) #define SPL06_MEAS_CFG_PRESSURE_RDY (1<<4) @@ -70,6 +75,13 @@ extern const AP_HAL::HAL &hal; #define SPL06_OVERSAMPLING_TO_REG_VALUE(n) (ffs(n)-1) +#define SPL06_BACKGROUND_SAMPLE_RATE 32 + +// enable Background Mode for continuous measurement +#ifndef AP_BARO_SPL06_BACKGROUND_ENABLE +#define AP_BARO_SPL06_BACKGROUND_ENABLE 0 +#endif + AP_Baro_SPL06::AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr dev) : AP_Baro_Backend(baro) , _dev(std::move(dev)) @@ -108,21 +120,43 @@ bool AP_Baro_SPL06::_init() // Sometimes SPL06 has init problems, that's due to failure of reading using SPI for the first time. The SPL06 is a dual // protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up. - bool is_SPL06 = false; for (uint8_t i=0; i<5; i++) { - if (_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1) && - whoami == SPL06_CHIP_ID) { - is_SPL06=true; - break; + if (_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1)) { + switch(whoami) { + case SPL06_CHIP_ID: + type = Type::SPL06; + break; + case SPA06_CHIP_ID: + type = Type::SPA06; + break; + default: + type = Type::UNKNOWN; + break; + } } + + if (type != Type::UNKNOWN) + break; } - if(!is_SPL06) { + if (type == Type::UNKNOWN) { return false; } // read the calibration data + uint8_t SPL06_CALIB_COEFFS_LEN = 18; + switch(type) { + case Type::SPL06: + SPL06_CALIB_COEFFS_LEN = SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1; + break; + case Type::SPA06: + SPL06_CALIB_COEFFS_LEN = SPA06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1; + break; + default: + break; + } + uint8_t buf[SPL06_CALIB_COEFFS_LEN]; _dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf)); @@ -135,12 +169,25 @@ bool AP_Baro_SPL06::_init() _c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13]; _c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15]; _c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17]; + if(type == Type::SPA06) { + _c31 = (buf[18] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[18] << 4) | (((uint16_t)buf[19] & 0xF0) >> 4); + _c40 = ((buf[19] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[19] & 0x0F) << 8) | (uint16_t)buf[20]; + } // setup temperature and pressure measurements _dev->setup_checked_registers(3, 20); +#if AP_BARO_SPL06_BACKGROUND_ENABLE + //set rate and oversampling + _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); + _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_PRES_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true); + + //enable background mode + _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_CON_PRE_TEM, true); +#else _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true); +#endif //AP_BARO_SPL06_BACKGROUND_ENABLE uint8_t int_and_fifo_reg_value = 0; if (SPL06_TEMPERATURE_OVERSAMPLING > 8) { @@ -158,7 +205,11 @@ bool AP_Baro_SPL06::_init() // request 50Hz update _timer_counter = -1; +#if AP_BARO_SPL06_BACKGROUND_ENABLE + _dev->register_periodic_callback(1000000/SPL06_BACKGROUND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void)); +#else _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void)); +#endif //AP_BARO_SPL06_BACKGROUND_ENABLE return true; } @@ -185,7 +236,15 @@ void AP_Baro_SPL06::_timer(void) { uint8_t buf[3]; - if ((_timer_counter == -1) || (_timer_counter == 49)) { +#if AP_BARO_SPL06_BACKGROUND_ENABLE + _dev->read_registers(SPL06_REG_TEMPERATURE_START, buf, sizeof(buf)); + _update_temperature((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2])); + + _dev->read_registers(SPL06_REG_PRESSURE_START, buf, sizeof(buf)); + _update_pressure((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2])); +#else + //command mode + if ((_timer_counter == -1) || (_timer_counter == 49)) { // First call and every second start a temperature measurement (50Hz call) _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_TEMPERATURE, false); _timer_counter = 0; // Next cycle we are reading the temperature @@ -202,6 +261,7 @@ void AP_Baro_SPL06::_timer(void) _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false); _timer_counter += 1; } +#endif //AP_BARO_SPL06_BACKGROUND_ENABLE _dev->check_next_register(); } @@ -236,8 +296,21 @@ void AP_Baro_SPL06::_update_temperature(int32_t temp_raw) void AP_Baro_SPL06::_update_pressure(int32_t press_raw) { const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING); - const float pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30)); - const float press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21)); + float pressure_cal = 0; + float press_temp_comp = 0; + + switch(type) { + case Type::SPL06: + pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30)); + press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21)); + break; + case Type::SPA06: + pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * ((float)_c30 + press_raw_sc * _c40))); + press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * ((float)_c21) + press_raw_sc * _c31)); + break; + default: + break; + } const float press_comp = pressure_cal + press_temp_comp; diff --git a/libraries/AP_Baro/AP_Baro_SPL06.h b/libraries/AP_Baro/AP_Baro_SPL06.h index 7dcba34ee804c4..1cdda4762fd4cf 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.h +++ b/libraries/AP_Baro/AP_Baro_SPL06.h @@ -18,6 +18,11 @@ class AP_Baro_SPL06 : public AP_Baro_Backend { public: + enum class Type { + UNKNOWN, + SPL06, + SPA06, + }; AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr dev); /* AP_Baro public interface: */ @@ -45,7 +50,9 @@ class AP_Baro_SPL06 : public AP_Baro_Backend // Internal calibration registers int32_t _c00, _c10; - int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30; + int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30, _c31, _c40; + + Type type; }; #endif // AP_BARO_SPL06_ENABLED diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index c37243723ed439..06a1147dc2adab 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -461,6 +461,12 @@ bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message ms CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS); send_camera_capture_status(chan); break; +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + case MSG_CAMERA_THERMAL_RANGE: + CHECK_PAYLOAD_SIZE2(CAMERA_THERMAL_RANGE); + send_camera_thermal_range(chan); + break; +#endif default: // should not reach this; should only be called for specific IDs @@ -615,6 +621,21 @@ void AP_Camera::send_camera_capture_status(mavlink_channel_t chan) } } +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +// send camera thermal range message to GCS +void AP_Camera::send_camera_thermal_range(mavlink_channel_t chan) +{ + WITH_SEMAPHORE(_rsem); + + // call each instance + for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) { + if (_backends[instance] != nullptr) { + _backends[instance]->send_camera_thermal_range(chan); + } + } +} +#endif + /* update; triggers by distance moved and camera trigger */ diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 28a27b0fa36b65..37f33942273fe7 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -240,6 +240,11 @@ class AP_Camera { // send camera capture status message to GCS void send_camera_capture_status(mavlink_channel_t chan); +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + // send camera thermal range message to GCS + void send_camera_thermal_range(mavlink_channel_t chan); +#endif + HAL_Semaphore _rsem; // semaphore for multi-thread access AP_Camera_Backend *primary; // primary camera backed bool _is_in_auto_mode; // true if in AUTO mode diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index d52944f5c0e517..c9b55949e61db3 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -126,6 +126,11 @@ class AP_Camera_Backend // send camera capture status message to GCS virtual void send_camera_capture_status(mavlink_channel_t chan) const; +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + // send camera thermal range message to GCS + virtual void send_camera_thermal_range(mavlink_channel_t chan) const {}; +#endif + #if AP_CAMERA_SCRIPTING_ENABLED // accessor to allow scripting backend to retrieve state // returns true on success and cam_state is filled in diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index b472d068eb5d64..9599097d8ce1c1 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -109,6 +109,19 @@ void AP_Camera_Mount::send_camera_capture_status(mavlink_channel_t chan) const } } +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +// send camera thermal range message to GCS +void AP_Camera_Mount::send_camera_thermal_range(mavlink_channel_t chan) const +{ +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + AP_Mount* mount = AP::mount(); + if (mount != nullptr) { + mount->send_camera_thermal_range(get_mount_instance(), chan); + } +#endif +} +#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + #if AP_CAMERA_SCRIPTING_ENABLED // change camera settings not normally used by autopilot bool AP_Camera_Mount::change_setting(CameraSetting setting, float value) diff --git a/libraries/AP_Camera/AP_Camera_Mount.h b/libraries/AP_Camera/AP_Camera_Mount.h index b3dd450652d145..19e56ff3c078b1 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.h +++ b/libraries/AP_Camera/AP_Camera_Mount.h @@ -70,6 +70,11 @@ class AP_Camera_Mount : public AP_Camera_Backend // send camera capture status message to GCS void send_camera_capture_status(mavlink_channel_t chan) const override; +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + // send camera thermal range message to GCS + void send_camera_thermal_range(mavlink_channel_t chan) const override; +#endif + #if AP_CAMERA_SCRIPTING_ENABLED // change camera settings not normally used by autopilot bool change_setting(CameraSetting setting, float value) override; diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 24b51275be72cf..6ff54ec978b57b 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -50,6 +50,11 @@ #define AP_CAMERA_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED #endif +// send thermal range is supported on a few thermal cameras but all are within mounts +#ifndef AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +#define AP_CAMERA_SEND_THERMAL_RANGE_ENABLED AP_MOUNT_SEND_THERMAL_RANGE_ENABLED +#endif + #ifndef HAL_RUNCAM_ENABLED #define HAL_RUNCAM_ENABLED 1 #endif diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index 9ae349e753aeab..eae21f7c933807 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp @@ -138,8 +138,6 @@ uint8_t AP_DroneCAN_DNA_Server::Database::handle_allocation(uint8_t node_id, con resp_node_id = find_free_node_id(node_id > MAX_NODE_ID ? 0 : node_id); if (resp_node_id != 0) { create_registration(resp_node_id, unique_id, 16); - } else { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!"); } } return resp_node_id; // will be 0 if not found and not created @@ -444,68 +442,61 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co } } -/* Handle the allocation message from the devices supporting -dynamic node allocation. */ -void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg) +// process node ID allocation messages for DNA +void AP_DroneCAN_DNA_Server::handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg) { if (transfer.source_node_id != 0) { - //Ignore Allocation messages that are not DNA requests - return; + return; // ignore allocation messages that are not DNA requests } uint32_t now = AP_HAL::millis(); if (rcvd_unique_id_offset == 0 || - (now - last_alloc_msg_ms) > 500) { + (now - last_alloc_msg_ms) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) { if (msg.first_part_of_unique_id) { rcvd_unique_id_offset = 0; - memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); } else { - //we are only accepting first part - return; + return; // only accepting the first part } } else if (msg.first_part_of_unique_id) { - // we are only accepting follow up messages - return; + return; // only accepting follow up messages } if (rcvd_unique_id_offset) { - debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting Followup part! %u\n", - (long int)AP_HAL::millis(), + debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %lu -- Accepting Followup part! %u\n", + (unsigned long)now, unsigned((now - last_alloc_msg_ms))); } else { - debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting First part! %u\n", - (long int)AP_HAL::millis(), + debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %lu -- Accepting First part! %u\n", + (unsigned long)now, unsigned((now - last_alloc_msg_ms))); } last_alloc_msg_ms = now; - if ((rcvd_unique_id_offset + msg.unique_id.len) > 16) { - //This request is malformed, Reset! - rcvd_unique_id_offset = 0; - memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); + if ((rcvd_unique_id_offset + msg.unique_id.len) > sizeof(rcvd_unique_id)) { + rcvd_unique_id_offset = 0; // reset state, request contains an over-long ID return; } - //copy over the unique_id - for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) { - rcvd_unique_id[i] = msg.unique_id.data[i - rcvd_unique_id_offset]; - } + // save the new portion of the unique ID + memcpy(&rcvd_unique_id[rcvd_unique_id_offset], msg.unique_id.data, msg.unique_id.len); rcvd_unique_id_offset += msg.unique_id.len; - //send follow up message + // respond with the message containing the received unique ID so far, or + // with the node ID if we successfully allocated one uavcan_protocol_dynamic_node_id_Allocation rsp {}; - - /* Respond with the message containing the received unique ID so far - or with node id if we successfully allocated one. */ memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset); rsp.unique_id.len = rcvd_unique_id_offset; - if (rcvd_unique_id_offset == 16) { - //We have received the full Unique ID, time to do allocation - rsp.node_id = db.handle_allocation(msg.node_id, (const uint8_t*)rcvd_unique_id); - //reset states as well - rcvd_unique_id_offset = 0; - memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); + if (rcvd_unique_id_offset == sizeof(rcvd_unique_id)) { // full unique ID received, allocate it! + rsp.node_id = db.handle_allocation(msg.node_id, rcvd_unique_id); + rcvd_unique_id_offset = 0; // reset state for next allocation + if (rsp.node_id == 0) { // allocation failed + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN DNA allocation failed; database full"); + // don't send reply with a failed ID in case the allocatee does + // silly things, though it is technically legal. the allocatee will + // then time out and try again (though we still won't have an ID!) + return; + } } allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h index 9557d28ace5dad..19f9af251c71ff 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h @@ -123,7 +123,7 @@ class AP_DroneCAN_DNA_Server char fault_node_name[15]; - //Allocation params + // dynamic node ID allocation state variables uint8_t rcvd_unique_id[16]; uint8_t rcvd_unique_id_offset; uint32_t last_alloc_msg_ms; @@ -133,7 +133,7 @@ class AP_DroneCAN_DNA_Server Canard::Publisher allocation_pub{_canard_iface}; - Canard::ObjCallback allocation_cb{this, &AP_DroneCAN_DNA_Server::handleAllocation}; + Canard::ObjCallback allocation_cb{this, &AP_DroneCAN_DNA_Server::handle_allocation}; Canard::Subscriber allocation_sub; Canard::ObjCallback node_status_cb{this, &AP_DroneCAN_DNA_Server::handleNodeStatus}; @@ -159,8 +159,8 @@ class AP_DroneCAN_DNA_Server //report the server state, along with failure message if any bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const; - //Callbacks - void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg); + // canard message handler callbacks + void handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg); void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg); void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp); diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp index 56102a4d305dba..bf5b94edaa22ac 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp @@ -86,7 +86,20 @@ void AP_DroneCAN_Serial::update(void) } n = MIN(avail, sizeof(pkt.buffer.data)); pkt.target_node = p.node; - pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED; + switch (p.state.protocol) { + case AP_SerialManager::SerialProtocol_MAVLink: + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_MAVLINK; + break; + case AP_SerialManager::SerialProtocol_MAVLink2: + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_MAVLINK2; + break; + case AP_SerialManager::SerialProtocol_GPS: + case AP_SerialManager::SerialProtocol_GPS2: // is not in SERIAL1_PROTOCOL option list, but could be entered by user + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_GPS_GENERIC; + break; + default: + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED; + } pkt.buffer.len = n; pkt.baudrate = p.baudrate; pkt.serial_id = p.idx; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 02c2f8f205ed0d..17f62791526b91 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -568,16 +568,16 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem #if AP_EXTENDED_ESC_TELEM_ENABLED if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) { - _telem_data[esc_index].input_duty = new_data.input_duty; + telemdata.input_duty = new_data.input_duty; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) { - _telem_data[esc_index].output_duty = new_data.output_duty; + telemdata.output_duty = new_data.output_duty; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) { - _telem_data[esc_index].flags = new_data.flags; + telemdata.flags = new_data.flags; } if (data_mask & AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE) { - _telem_data[esc_index].power_percentage = new_data.power_percentage; + telemdata.power_percentage = new_data.power_percentage; } #endif //AP_EXTENDED_ESC_TELEM_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 229ca00dea23f2..9abbfff0d1a97c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -180,6 +180,17 @@ bool AP_ExternalAHRS::get_origin(Location &loc) return false; } +bool AP_ExternalAHRS::set_origin(const Location &loc) +{ + WITH_SEMAPHORE(state.sem); + if (state.have_origin) { + return false; + } + state.origin = loc; + state.have_origin = true; + return true; +} + bool AP_ExternalAHRS::get_location(Location &loc) { if (!state.have_location) { @@ -312,19 +323,7 @@ void AP_ExternalAHRS::update(void) backend->update(); } - /* - if backend has not supplied an origin and AHRS has an origin - then use that origin so we get a common origin for minimum - disturbance when switching backends - */ WITH_SEMAPHORE(state.sem); - if (!state.have_origin) { - Location origin; - if (AP::ahrs().get_origin(origin)) { - state.origin = origin; - state.have_origin = true; - } - } #if HAL_LOGGING_ENABLED const uint32_t now_ms = AP_HAL::millis(); if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index a08c2822914b1e..22ef0e9d5f8299 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -109,6 +109,7 @@ class AP_ExternalAHRS { bool initialised(void) const; bool get_quaternion(Quaternion &quat); bool get_origin(Location &loc); + bool set_origin(const Location &loc); bool get_location(Location &loc); Vector2f get_groundspeed_vector(); bool get_velocity_NED(Vector3f &vel); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat index ded7e7d7a51db9..b576cfdb530a0c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat @@ -23,6 +23,10 @@ define HAL_PERIPH_GPS_PORT_DEFAULT 2 # allow for F9P GPS modules with moving baseline define GPS_MOVING_BASELINE 1 +# restrict backends available to save flash (i.e. only UBLOX) +define AP_GPS_NOVA_ENABLED 0 +define AP_GPS_SBF_ENABLED 0 +define AP_GPS_GSOF_ENABLED 0 # ---------------------- COMPASS --------------------------- define HAL_PERIPH_ENABLE_MAG diff --git a/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat index 82c7d676b1c446..c7edef50b09dbd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat @@ -187,7 +187,6 @@ COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_YAW_180 # one baro BARO SPL06 I2C:0:0x76 -BARO SPA06 I2C:0:0x76 # microSD support PC8 SDMMC_D0 SDMMC1 diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index 4f22e6e42d5317..84cca6e1362c42 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -1068,17 +1067,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) #if AP_RC_CHANNEL_ENABLED MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) { -#if AP_RCMAPPER_ENABLED - const RCMapper* rcmap = AP::rcmap(); - if (rcmap == nullptr) { - return MSP_RESULT_ERROR; - } - - // note: rcmap channels start at 1 - float roll = rc().rc_channel(rcmap->roll()-1)->norm_input_dz(); - float pitch = -rc().rc_channel(rcmap->pitch()-1)->norm_input_dz(); - float yaw = rc().rc_channel(rcmap->yaw()-1)->norm_input_dz(); - float throttle = rc().rc_channel(rcmap->throttle()-1)->norm_input_dz(); + float roll = rc().get_roll_channel().norm_input_dz(); + float pitch = -rc().get_pitch_channel().norm_input_dz(); + float yaw = rc().get_yaw_channel().norm_input_dz(); + float throttle = rc().get_throttle_channel().norm_input_dz(); const struct PACKED { uint16_t a; @@ -1095,9 +1087,6 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) sbuf_write_data(dst, &rc, sizeof(rc)); return MSP_RESULT_ACK; -#else - return MSP_RESULT_ERROR; -#endif } #endif // AP_RC_CHANNEL_ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c88856d40f9892..fe7f29f7eac3c1 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -907,6 +907,18 @@ void AP_Mount::send_camera_capture_status(uint8_t instance, mavlink_channel_t ch backend->send_camera_capture_status(chan); } +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED +// send camera thermal range message to GCS +void AP_Mount::send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->send_camera_thermal_range(chan); +} +#endif + // change camera settings not normally used by autopilot // setting values from AP_Camera::Setting enum bool AP_Mount::change_setting(uint8_t instance, CameraSetting setting, float value) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 8db5c31a006894..54762e915307fc 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -271,6 +271,11 @@ class AP_Mount // send camera capture status message to GCS void send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const; +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // send camera thermal range message to GCS + void send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const; +#endif + // change camera settings not normally used by autopilot // setting values from AP_Camera::Setting enum bool change_setting(uint8_t instance, CameraSetting setting, float value); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 28e7dfd478fece..16082d58f450b5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -198,6 +198,11 @@ class AP_Mount_Backend // send camera capture status message to GCS virtual void send_camera_capture_status(mavlink_channel_t chan) const {} +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // send camera thermal status message to GCS + virtual void send_camera_thermal_range(mavlink_channel_t chan) const {} +#endif + // change camera settings not normally used by autopilot virtual bool change_setting(CameraSetting setting, float value) { return false; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index c89b8cb676e48c..1c3c8cf8316d92 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -17,6 +17,7 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_SIYI_PITCH_P 1.50 // pitch controller P gain (converts pitch angle error to target rate) #define AP_MOUNT_SIYI_YAW_P 1.50 // yaw controller P gain (converts yaw angle error to target rate) #define AP_MOUNT_SIYI_TIMEOUT_MS 1000 // timeout for health and rangefinder readings +#define AP_MOUNT_SIYI_THERM_TIMEOUT_MS 3000// timeout for temp min/max readings #define AP_MOUNT_SIYI_DEBUG 0 #define debug(fmt, args ...) do { if (AP_MOUNT_SIYI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0) @@ -82,6 +83,11 @@ void AP_Mount_Siyi::update() _last_rangefinder_req_ms = now_ms; } +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // request thermal min/max from ZT30 or ZT6 + request_thermal_minmax(); +#endif + // send attitude to gimbal at 10Hz if (now_ms - _last_attitude_send_ms > 100) { _last_attitude_send_ms = now_ms; @@ -544,6 +550,25 @@ void AP_Mount_Siyi::process_packet() break; } +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + case SiyiCommandId::GET_TEMP_FULL_IMAGE: { + if (_parsed_msg.data_bytes_received != 12) { +#if AP_MOUNT_SIYI_DEBUG + unexpected_len = true; +#endif + break; + } + _thermal.last_update_ms = AP_HAL::millis(); + _thermal.max_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.01; + _thermal.min_C = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.01; + _thermal.max_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]); + _thermal.max_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]); + _thermal.min_pos.x = UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]); + _thermal.min_pos.y = UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]); + break; + } +#endif + case SiyiCommandId::READ_RANGEFINDER: { _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]); _last_rangefinder_dist_ms = AP_HAL::millis(); @@ -1098,6 +1123,29 @@ void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown } +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED +// send camera thermal range message to GCS +void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const +{ + const float NaN = nanf("0x4152"); + const uint32_t now_ms = AP_HAL::millis(); + bool timeout = now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS; + + // send CAMERA_THERMAL_RANGE message + mavlink_msg_camera_thermal_range_send( + chan, + now_ms, // time_boot_ms + _instance + 1, // video stream id (assume one-to-one mapping with camera id) + _instance + 1, // camera device id + timeout ? NaN : _thermal.max_C, // max in degC + timeout ? NaN : _thermal.max_pos.x, // max x position + timeout ? NaN : _thermal.max_pos.y, // max y position + timeout ? NaN : _thermal.min_C, // min in degC + timeout ? NaN : _thermal.min_pos.x, // min x position + timeout ? NaN : _thermal.min_pos.y);// min y position +} +#endif + // change camera settings not normally used by autopilot // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C) @@ -1193,6 +1241,27 @@ void AP_Mount_Siyi::check_firmware_version() const } } +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED +// get thermal min/max if available at 5hz +void AP_Mount_Siyi::request_thermal_minmax() +{ + // only supported on ZT6 and ZT30 + if (_hardware_model != HardwareModel::ZT6 && + _hardware_model != HardwareModel::ZT30) { + return; + } + + // check for timeout + uint32_t now_ms = AP_HAL::millis(); + if ((now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS) && + (now_ms - _thermal.last_req_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS)) { + // request thermal min/max at 5hz + send_1byte_packet(SiyiCommandId::GET_TEMP_FULL_IMAGE, 2); + _thermal.last_req_ms = now_ms; + } +} +#endif + /* send ArduPilot attitude and position to gimbal */ diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index bef712e5fd585b..20285b0b5bd6c1 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -82,6 +82,11 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial // send camera settings message to GCS void send_camera_settings(mavlink_channel_t chan) const override; +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // send camera thermal range message to GCS + void send_camera_thermal_range(mavlink_channel_t chan) const override; +#endif + // change camera settings not normally used by autopilot // THERMAL_PALETTE: 0:WhiteHot, 2:Sepia, 3:IronBow, 4:Rainbow, 5:Night, 6:Aurora, 7:RedHot, 8:Jungle, 9:Medical, 10:BlackHot, 11:GloryHot // THERMAL_GAIN: 0:Low gain (50C ~ 550C), 1:High gain (-20C ~ 150C) @@ -123,6 +128,7 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial ACQUIRE_GIMBAL_ATTITUDE = 0x0D, ABSOLUTE_ZOOM = 0x0F, SET_CAMERA_IMAGE_TYPE = 0x11, + GET_TEMP_FULL_IMAGE = 0x14, READ_RANGEFINDER = 0x15, SET_THERMAL_PALETTE = 0x1B, EXTERNAL_ATTITUDE = 0x22, @@ -300,6 +306,11 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial // Checks that the firmware version on the Gimbal meets the minimum supported version. void check_firmware_version() const; +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // get thermal min/max if available at 5hz + void request_thermal_minmax(); +#endif + // internal variables bool _got_hardware_id; // true once hardware id ha been received @@ -354,6 +365,18 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial }; static const HWInfo hardware_lookup_table[]; +#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED + // thermal variables + struct { + uint32_t last_req_ms; // system time of last request for thermal min/max temperatures + uint32_t last_update_ms; // system time of last update of thermal min/max temperatures + float max_C; // thermal max temp in C + float min_C; // thermal min temp in C + Vector2ui max_pos; // thermal max temp position on image in pixels. x=0 is left, y=0 is top + Vector2ui min_pos; // thermal min temp position on image in pixels. x=0 is left, y=0 is top + } _thermal; +#endif + // count of SET_TIME packets, we send 5 times to cope with packet loss uint8_t sent_time_count; }; diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index a2da37fac58805..f40a39cd05b063 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -64,3 +64,8 @@ #ifndef HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED #define HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SIYI_ENABLED || HAL_MOUNT_XACTI_ENABLED || HAL_MOUNT_VIEWPRO_ENABLED #endif + +// send thermal range is only support on Siyi cameras +#ifndef AP_MOUNT_SEND_THERMAL_RANGE_ENABLED +#define AP_MOUNT_SEND_THERMAL_RANGE_ENABLED HAL_MOUNT_SIYI_ENABLED +#endif diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index dfd2aa7b6d3abb..98c8e58a6ca758 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1099,6 +1099,10 @@ bool NavEKF2::getOriginLLH(Location &loc) const if (!core) { return false; } + if (common_origin_valid) { + loc = common_EKF_origin; + return true; + } return core[primary].getOriginLLH(loc); } @@ -1113,11 +1117,9 @@ bool NavEKF2::setOriginLLH(const Location &loc) if (!core) { return false; } - if (_fusionModeGPS != 3 || common_origin_valid) { - // we don't allow setting of the EKF origin if using GPS - // or if the EKF origin has already been set. - // This is to prevent accidental setting of EKF origin with an - // invalid position or height or causing upsets from a shifting origin. + if (common_origin_valid) { + // we don't allow setting of the EKF origin if the EKF origin + // has already been set. GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 refusing set origin"); return false; } diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 5b66134957849e..7a5dbad3cfe770 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -2,7 +2,7 @@ #include #if defined(AP_NETWORKING_BACKEND_PPP) && !defined(AP_NETWORKING_ENABLED) -// allow --enable-ppp to enable networking +// allow --enable-PPP to enable networking #define AP_NETWORKING_ENABLED AP_NETWORKING_BACKEND_PPP #endif diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h index deee2b77df1eaf..4ccda3610d89e3 100644 --- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h @@ -21,9 +21,7 @@ class AP_Proximity_DroneCAN : public AP_Proximity_Backend float distance_max() const override; float distance_min() const override; - - static AP_Proximity_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new); - + static AP_Proximity_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new); static void subscribe_msgs(AP_DroneCAN* ap_dronecan); diff --git a/libraries/AP_Scripting/AP_Scripting_config.h b/libraries/AP_Scripting/AP_Scripting_config.h index 77f9dc8d76ce35..768e751ca7bb1d 100644 --- a/libraries/AP_Scripting/AP_Scripting_config.h +++ b/libraries/AP_Scripting/AP_Scripting_config.h @@ -9,7 +9,8 @@ #if AP_SCRIPTING_ENABLED #include - #if !AP_FILESYSTEM_FILE_READING_ENABLED + // enumerate all of the possible places we can read a script from. + #if !AP_FILESYSTEM_POSIX_ENABLED && !AP_FILESYSTEM_FATFS_ENABLED && !AP_FILESYSTEM_ESP32_ENABLED && !AP_FILESYSTEM_ROMFS_ENABLED #error "Scripting requires a filesystem" #endif #endif diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index a3e587c53e6a0b..03c94921136f9d 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -255,10 +255,13 @@ void lua_scripts::load_all_scripts_in_dir(lua_State *L, const char *dirname) { if (dirname == nullptr) { return; } - auto *d = AP::FS().opendir(dirname); if (d == nullptr) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Lua: open directory (%s) failed", dirname); + // this disk_space check will return 0 if we don't have a real + // filesystem (ie. no Posix or FatFs). Do not warn in this case. + if (AP::FS().disk_space(dirname) != 0) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Lua: open directory (%s) failed", dirname); + } return; } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c546639ecc5a7c..75ae487b11ec8e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1060,7 +1060,10 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS}, { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS}, { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS}, -#endif +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + { MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, MSG_CAMERA_THERMAL_RANGE}, +#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED +#endif // AP_CAMERA_ENABLED #if HAL_MOUNT_ENABLED { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE}, @@ -3029,6 +3032,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_int_t &packet) { + if (!is_zero(packet.param3)) { + return MAV_RESULT_DENIED; + } return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2); } @@ -6128,6 +6134,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) case MSG_CAMERA_FOV_STATUS: #endif case MSG_CAMERA_CAPTURE_STATUS: +#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED + case MSG_CAMERA_THERMAL_RANGE: +#endif { AP_Camera *camera = AP::camera(); if (camera == nullptr) { diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 4923317266025c..2d6761db8f33cb 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -6,6 +6,8 @@ #pragma once +#include "GCS_config.h" + #include enum ap_message : uint8_t { @@ -57,6 +59,7 @@ enum ap_message : uint8_t { MSG_CAMERA_SETTINGS, MSG_CAMERA_FOV_STATUS, MSG_CAMERA_CAPTURE_STATUS, + MSG_CAMERA_THERMAL_RANGE, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_STATUS, diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 34dfb90e92638e..6e5c766e052c9d 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -612,6 +612,12 @@ class RC_Channels { // get failsafe timeout in milliseconds uint32_t get_fs_timeout_ms() const { return MAX(_fs_timeout * 1000, 100); } + // methods which return RC input channels used for various axes. + RC_Channel &get_roll_channel(); + RC_Channel &get_pitch_channel(); + RC_Channel &get_yaw_channel(); + RC_Channel &get_throttle_channel(); + protected: void new_override_received() { @@ -653,6 +659,9 @@ class RC_Channels { void set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwitchPos pos); #endif + + RC_Channel &get_rcmap_channel_nonnull(uint8_t rcmap_number); + RC_Channel dummy_rcchannel; }; RC_Channels &rc(); diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 9ec73c5dab365e..cd32cf938ba3bd 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -30,6 +30,7 @@ extern const AP_HAL::HAL& hal; #include #include +#include #include "RC_Channel.h" @@ -307,6 +308,39 @@ void RC_Channels::set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwi } #endif // AP_SCRIPTING_ENABLED +#if AP_RCMAPPER_ENABLED +// these methods return an RC_Channel pointers based on values from +// AP_::rcmap(). The return value is guaranteed to be not-null to +// allow use of the pointer without checking it for null-ness. If an +// invalid option has been chosen somehow then the returned channel +// will be a dummy channel. +RC_Channel &RC_Channels::get_rcmap_channel_nonnull(uint8_t rcmap_number) +{ + RC_Channel *ret = RC_Channels::rc_channel(rcmap_number-1); + if (ret != nullptr) { + return *ret; + } + return dummy_rcchannel; +} +RC_Channel &RC_Channels::get_roll_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->roll()); +}; +RC_Channel &RC_Channels::get_pitch_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->pitch()); +}; +RC_Channel &RC_Channels::get_throttle_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->throttle()); +}; +RC_Channel &RC_Channels::get_yaw_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->yaw()); +}; +#endif // AP_RCMAPPER_ENABLED + + // singleton instance RC_Channels *RC_Channels::_singleton; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index ee9d59f28183f6..6d85b5059d7064 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -65,8 +65,14 @@ SIM *SIM::_singleton = nullptr; // table of user settable parameters const AP_Param::GroupInfo SIM::var_info[] = { - + + // @Param: DRIFT_SPEED + // @DisplayName: Gyro drift speed + // @Description: Gyro drift rate of change in degrees/second/minute AP_GROUPINFO("DRIFT_SPEED", 5, SIM, drift_speed, 0.05f), + // @Param: DRIFT_TIME + // @DisplayName: Gyro drift time + // @Description: Gyro drift duration of one full drift cycle (period in minutes) AP_GROUPINFO("DRIFT_TIME", 6, SIM, drift_time, 5), // @Param: ENGINE_MUL // @DisplayName: Engine failure thrust scaler @@ -111,6 +117,11 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Units: V // @User: Advanced AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, 12.6f), + // @Param: BATT_CAP_AH + // @DisplayName: Simulated battery capacity + // @Description: Simulated battery capacity + // @Units: Ah + // @User: Advanced AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0), AP_GROUPINFO("SONAR_GLITCH", 23, SIM, sonar_glitch, 0), AP_GROUPINFO("SONAR_RND", 24, SIM, sonar_noise, 0), @@ -168,10 +179,25 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Description: Opflow data delay // @Units: ms AP_GROUPINFO("FLOW_DELAY", 36, SIM, flow_delay, 0), + // @Param: ADSB_COUNT + // @DisplayName: Number of ADSB aircrafts + // @Description: Total number of ADSB simulated aircraft AP_GROUPINFO("ADSB_COUNT", 45, SIM, adsb_plane_count, -1), + // @Param: ADSB_RADIUS + // @DisplayName: ADSB radius stddev of another aircraft + // @Description: Simulated standard deviation of radius in ADSB of another aircraft + // @Units: m AP_GROUPINFO("ADSB_RADIUS", 46, SIM, adsb_radius_m, 10000), + // @Param: ADSB_ALT + // @DisplayName: ADSB altitude of another aircraft + // @Description: Simulated ADSB altitude of another aircraft + // @Units: m AP_GROUPINFO("ADSB_ALT", 47, SIM, adsb_altitude_m, 1000), AP_GROUPINFO("PIN_MASK", 50, SIM, pin_mask, 0), + // @Param: ADSB_TX + // @DisplayName: ADSB transmit enable + // @Description: ADSB transceiever enable and disable + // @Values: 0:Transceiever disable, 1:Transceiever enable AP_GROUPINFO("ADSB_TX", 51, SIM, adsb_tx, 0), // @Param: SPEEDUP // @DisplayName: Sim Speedup @@ -261,7 +287,9 @@ const AP_Param::GroupInfo SIM::var_info2[] = { // @Path: ./SIM_Parachute.cpp AP_SUBGROUPINFO(parachute_sim, "PARA_", 27, SIM, Parachute), - // enable bandwidth limitting on telemetry ports: + // @Param: BAUDLIMIT_EN + // @DisplayName: Telemetry bandwidth limitting + // @Description: SITL enable bandwidth limitting on telemetry ports with non-zero values AP_GROUPINFO("BAUDLIMIT_EN", 28, SIM, telem_baudlimit_enable, 0), // @Group: PLD_ @@ -327,6 +355,10 @@ const AP_Param::GroupInfo SIM::var_info2[] = { // @Path: ./SIM_ToneAlarm.cpp AP_SUBGROUPINFO(tonealarm_sim, "TA_", 57, SIM, ToneAlarm), + // @Param: EFI_TYPE + // @DisplayName: Type of Electronic Fuel Injection + // @Description: Different types of Electronic Fuel Injection (EFI) systems + // @Values: 0:None,1:MegaSquirt EFI system, 2:Löweheiser EFI system, 8:Hirth engines AP_GROUPINFO("EFI_TYPE", 58, SIM, efi_type, SIM::EFI_TYPE_NONE), // 59 was SAFETY_STATE @@ -468,6 +500,10 @@ const AP_Param::GroupInfo SIM::var_info3[] = { AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SIM, IntelligentEnergy24), // user settable barometer parameters + + // @Param: BARO_COUNT + // @DisplayName: Baro count + // @Description: Total baro count AP_GROUPINFO("BARO_COUNT", 33, SIM, baro_count, 2), AP_SUBGROUPINFO(baro[0], "BARO_", 34, SIM, SIM::BaroParm), @@ -525,6 +561,10 @@ const AP_Param::GroupInfo SIM::var_info3[] = { // @User: Advanced AP_GROUPINFO("ESC_TELEM", 40, SIM, esc_telem, 1), + // @Param: ESC_ARM_RPM + // @DisplayName: ESC RPM when armed + // @Description: Simulated RPM when motors are armed + // @User: Advanced AP_GROUPINFO("ESC_ARM_RPM", 41, SIM, esc_rpm_armed, 0.0f), // @Param: UART_LOSS @@ -1043,8 +1083,16 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Description: channels which are passed through to actual hardware when running sim on actual hardware AP_GROUPINFO("OH_MASK", 28, SIM, on_hardware_output_enable_mask, 0), #if AP_SIM_INS_FILE_ENABLED - // read and write IMU data to/from files + + // @Param: GYR_FILE_RW + // @DisplayName: Gyro data to/from files + // @Description: Read and write gyro data to/from files + // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF AP_GROUPINFO("GYR_FILE_RW", 29, SIM, gyro_file_rw, INSFileMode::INS_FILE_NONE), + // @Param: ACC_FILE_RW + // @DisplayName: Accelerometer data to/from files + // @Description: Read and write accelerometer data to/from files + // @Values: 0:Stop writing data, 1:Read data from file, 2:Write data to a file, 3: Read data from file and stop on EOF AP_GROUPINFO("ACC_FILE_RW", 30, SIM, accel_file_rw, INSFileMode::INS_FILE_NONE), #endif diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 9e6d69b2b71a12..24631a448d2c41 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -188,6 +188,7 @@ class SRV_Channel { k_rcin14_mapped = 153, k_rcin15_mapped = 154, k_rcin16_mapped = 155, + k_lift_release = 156, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 1412957202c262..c117915e8a4744 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -140,6 +140,7 @@ void SRV_Channel::aux_servo_function_setup(void) case k_flap: case k_flap_auto: case k_egg_drop: + case k_lift_release: set_range(100); break; case k_heli_rsc: diff --git a/modules/mavlink b/modules/mavlink index 60aa4ff8472cf9..cdfe7a82380b4d 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 60aa4ff8472cf91506256082bea8a1c3bbf9068b +Subproject commit cdfe7a82380b4dae477cf5fac2fe0089817ac560 diff --git a/wscript b/wscript index 3d4012787ea5f6..89ab8961fb3214 100644 --- a/wscript +++ b/wscript @@ -10,10 +10,12 @@ import subprocess import json import fnmatch sys.path.insert(0, 'Tools/ardupilotwaf/') +sys.path.insert(0, 'Tools/scripts/') import ardupilotwaf import boards import shutil +import build_options from waflib import Build, ConfigSet, Configure, Context, Utils from waflib.Configure import conf @@ -204,11 +206,6 @@ def options(opt): default=False, help='enable OS level thread statistics.') - g.add_option('--enable-ppp', - action='store_true', - default=False, - help='enable PPP networking.') - g.add_option('--bootloader', action='store_true', default=False, @@ -388,16 +385,6 @@ configuration in order to save typing. default=False, help='Use flash storage emulation.') - g.add_option('--enable-ekf2', - action='store_true', - default=False, - help='Configure with EKF2.') - - g.add_option('--disable-ekf3', - action='store_true', - default=False, - help='Configure without EKF3.') - g.add_option('--ekf-double', action='store_true', default=False, @@ -446,6 +433,24 @@ configuration in order to save typing. action='store_true', default=False, help='enables checking of new to ensure NEW_NOTHROW is used') + + # support enabling any option in build_options.py + for opt in build_options.BUILD_OPTIONS: + enable_option = "--" + opt.config_option() + disable_option = enable_option.replace("--enable", "--disable") + enable_description = opt.description + if not enable_description.lower().startswith("enable"): + enable_description = "Enable " + enable_description + disable_description = "Disable " + enable_description[len("Enable "):] + g.add_option(enable_option, + action='store_true', + default=False, + help=enable_description) + g.add_option(disable_option, + action='store_true', + default=False, + help=disable_description) + def _collect_autoconfig_files(cfg): for m in sys.modules.values():