Merge branch 'master' into ti_bq_pr

ArduCopter/mode_circle.cpp

@@ -28,7 +28,7 @@ bool ModeCircle::init(bool ignore_checks)
     if (copter.circle_nav->roi_at_center()) {
         const Vector3p &pos { copter.circle_nav->get_center() };
         Location circle_center;
-        if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) {
+        if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos * 0.01)) {
             return false;
         }
         // point at the ground:

ArduCopter/mode_guided.cpp

@@ -243,10 +243,10 @@ void ModeGuided::pos_control_start()
     pva_control_start();
 }
 
-// initialise guided mode's velocity controller
+// initialise guided mode's acceleration controller
 void ModeGuided::accel_control_start()
 {
-    // set guided_mode to velocity controller
+    // set guided_mode to acceleration controller
     guided_mode = SubMode::Accel;
 
     // initialise position controller
@@ -256,7 +256,7 @@ void ModeGuided::accel_control_start()
 // initialise guided mode's velocity and acceleration controller
 void ModeGuided::velaccel_control_start()
 {
-    // set guided_mode to velocity controller
+    // set guided_mode to velocity and acceleration controller
     guided_mode = SubMode::VelAccel;
 
     // initialise position controller
@@ -266,7 +266,7 @@ void ModeGuided::velaccel_control_start()
 // initialise guided mode's position, velocity and acceleration controller
 void ModeGuided::posvelaccel_control_start()
 {
-    // set guided_mode to velocity controller
+    // set guided_mode to position, velocity and acceleration controller
     guided_mode = SubMode::PosVelAccel;
 
     // initialise position controller
@@ -414,11 +414,14 @@ bool ModeGuided::get_wp(Location& destination) const
     case SubMode::Pos:
         destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
         return true;
-    default:
-        return false;
+    case SubMode::Angle:
+    case SubMode::TakeOff:
+    case SubMode::Accel:
+    case SubMode::VelAccel:
+    case SubMode::PosVelAccel:
+        break;
     }
 
-    // should never get here but just in case
     return false;
 }
 
@@ -515,7 +518,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
 // set_velaccel - sets guided mode's target velocity and acceleration
 void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
 {
-    // check we are in velocity control mode
+    // check we are in acceleration control mode
     if (guided_mode != SubMode::Accel) {
         accel_control_start();
     }
@@ -547,7 +550,7 @@ void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_
 // set_velaccel - sets guided mode's target velocity and acceleration
 void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
 {
-    // check we are in velocity control mode
+    // check we are in velocity and acceleration control mode
     if (guided_mode != SubMode::VelAccel) {
         velaccel_control_start();
     }
@@ -589,7 +592,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
     }
 #endif
 
-    // check we are in velocity control mode
+    // check we are in position, velocity and acceleration control mode
     if (guided_mode != SubMode::PosVelAccel) {
         posvelaccel_control_start();
     }

Rover/Log.cpp

@@ -24,7 +24,7 @@ void Rover::Log_Write_Attitude()
     }
 
     // log heel to sail control for sailboats
-    if (rover.g2.sailboat.sail_enabled()) {
+    if (g2.sailboat.sail_enabled()) {
         logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info());
     }
 }
@@ -142,13 +142,13 @@ void Rover::Log_Write_Nav_Tuning()
 void Rover::Log_Write_Sail()
 {
     // only log sail if present
-    if (!rover.g2.sailboat.sail_enabled()) {
+    if (!g2.sailboat.sail_enabled()) {
         return;
     }
 
     float wind_dir_tack = logger.quiet_nanf();
     uint8_t current_tack = 0;
-    if (rover.g2.windvane.enabled()) {
+    if (g2.windvane.enabled()) {
         wind_dir_tack = degrees(g2.windvane.get_tack_threshold_wind_dir_rad());
         current_tack = uint8_t(g2.windvane.get_current_tack());
     }

Rover/mode.cpp

@@ -10,7 +10,7 @@ Mode::Mode() :
     channel_roll(rover.channel_roll),
     channel_pitch(rover.channel_pitch),
     channel_walking_height(rover.channel_walking_height),
-    attitude_control(rover.g2.attitude_control)
+    attitude_control(g2.attitude_control)
 { }
 
 void Mode::exit()
@@ -47,7 +47,7 @@ bool Mode::enter()
         set_reversed(false);
 
         // clear sailboat tacking flags
-        rover.g2.sailboat.clear_tack();
+        g2.sailboat.clear_tack();
     }
 
     return ret;
@@ -66,7 +66,7 @@ void Mode::get_pilot_input(float &steering_out, float &throttle_out) const
     }
 
     // apply RC skid steer mixing
-    switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get())
+    switch ((enum pilot_steer_type_t)g.pilot_steer_type.get())
     {
         case PILOT_STEER_TYPE_DEFAULT:
         case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING:
@@ -166,7 +166,7 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_
     float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f);
 
     // handle two paddle input
-    if ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) {
+    if ((enum pilot_steer_type_t)g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) {
         const float left_paddle = desired_steering;
         const float right_paddle = desired_throttle;
         desired_steering = (left_paddle - right_paddle) * 0.5f;
@@ -279,7 +279,7 @@ void Mode::handle_tack_request()
 {
     // autopilot modes handle tacking
     if (is_autopilot_mode()) {
-        rover.g2.sailboat.handle_tack_request_auto();
+        g2.sailboat.handle_tack_request_auto();
     }
 }
 
@@ -304,9 +304,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
     // call throttle controller and convert output to -100 to +100 range
     float throttle_out = 0.0f;
 
-    if (rover.g2.sailboat.sail_enabled()) {
+    if (g2.sailboat.sail_enabled()) {
         // sailboats use special throttle and mainsail controller
-        rover.g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out);
+        g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out);
     } else {
         // call speed or stop controller
         if (is_zero(target_speed) && !rover.is_balancebot()) {

Rover/mode_acro.cpp

@@ -28,12 +28,12 @@ void ModeAcro::update()
     // handle sailboats
     if (!is_zero(desired_steering)) {
         // steering input return control to user
-        rover.g2.sailboat.clear_tack();
+        g2.sailboat.clear_tack();
     }
-    if (rover.g2.sailboat.tacking()) {
+    if (g2.sailboat.tacking()) {
         // call heading controller during tacking
 
-        steering_out = attitude_control.get_steering_out_heading(rover.g2.sailboat.get_tack_heading_rad(),
+        steering_out = attitude_control.get_steering_out_heading(g2.sailboat.get_tack_heading_rad(),
                                                                  g2.wp_nav.get_pivot_rate(),
                                                                  g2.motors.limit.steer_left,
                                                                  g2.motors.limit.steer_right,
@@ -60,5 +60,5 @@ bool ModeAcro::requires_velocity() const
 // sailboats in acro mode support user manually initiating tacking from transmitter
 void ModeAcro::handle_tack_request()
 {
-    rover.g2.sailboat.handle_tack_request_acro();
+    g2.sailboat.handle_tack_request_acro();
 }

Rover/mode_auto.cpp

@@ -141,7 +141,7 @@ void ModeAuto::update()
             break;
 
         case SubMode::Circle:
-            rover.g2.mode_circle.update();
+            g2.mode_circle.update();
             break;
     }
 }
@@ -173,7 +173,7 @@ float ModeAuto::wp_bearing() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.wp_bearing();
     case SubMode::Circle:
-        return rover.g2.mode_circle.wp_bearing();
+        return g2.mode_circle.wp_bearing();
     }
 
     // this line should never be reached
@@ -197,7 +197,7 @@ float ModeAuto::nav_bearing() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.nav_bearing();
     case SubMode::Circle:
-        return rover.g2.mode_circle.nav_bearing();
+        return g2.mode_circle.nav_bearing();
     }
 
     // this line should never be reached
@@ -221,7 +221,7 @@ float ModeAuto::crosstrack_error() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.crosstrack_error();
     case SubMode::Circle:
-        return rover.g2.mode_circle.crosstrack_error();
+        return g2.mode_circle.crosstrack_error();
     }
 
     // this line should never be reached
@@ -245,7 +245,7 @@ float ModeAuto::get_desired_lat_accel() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.get_desired_lat_accel();
     case SubMode::Circle:
-        return rover.g2.mode_circle.get_desired_lat_accel();
+        return g2.mode_circle.get_desired_lat_accel();
     }
 
     // this line should never be reached
@@ -270,7 +270,7 @@ float ModeAuto::get_distance_to_destination() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.get_distance_to_destination();
     case SubMode::Circle:
-        return rover.g2.mode_circle.get_distance_to_destination();
+        return g2.mode_circle.get_distance_to_destination();
     }
 
     // this line should never be reached
@@ -299,7 +299,7 @@ bool ModeAuto::get_desired_location(Location& destination) const
     case SubMode::NavScriptTime:
         return rover.mode_guided.get_desired_location(destination);
     case SubMode::Circle:
-        return rover.g2.mode_circle.get_desired_location(destination);
+        return g2.mode_circle.get_desired_location(destination);
     }
 
     // we should never reach here but just in case
@@ -341,7 +341,7 @@ bool ModeAuto::reached_destination() const
     case SubMode::NavScriptTime:
         return rover.mode_guided.reached_destination();
     case SubMode::Circle:
-        return rover.g2.mode_circle.reached_destination();
+        return g2.mode_circle.reached_destination();
     }
 
     // we should never reach here but just in case, return true to allow missions to continue
@@ -366,7 +366,7 @@ bool ModeAuto::set_desired_speed(float speed)
     case SubMode::NavScriptTime:
         return rover.mode_guided.set_desired_speed(speed);
     case SubMode::Circle:
-        return rover.g2.mode_circle.set_desired_speed(speed);
+        return g2.mode_circle.set_desired_speed(speed);
     }
     return false;
 }
@@ -902,7 +902,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 
     // if a location target was set, return true once vehicle is close
     if (guided_target.valid) {
-        if (rover.current_loc.get_distance(guided_target.loc) <= rover.g2.wp_nav.get_radius()) {
+        if (rover.current_loc.get_distance(guided_target.loc) <= g2.wp_nav.get_radius()) {
             return true;
         }
     }

Rover/mode_circle.cpp

@@ -149,7 +149,7 @@ void ModeCircle::update()
     _distance_to_destination = center_to_veh.length();
     dist_to_edge_m = fabsf(_distance_to_destination - config.radius);
     if (!reached_edge) {
-        const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
+        const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
         reached_edge = dist_to_edge_m <= dist_thresh_m;
     }
 
@@ -266,8 +266,8 @@ void ModeCircle::check_config_speed()
 void ModeCircle::check_config_radius()
 {
     // ensure radius is at least as large as vehicle's turn radius
-    if (config.radius < rover.g2.turn_radius) {
-        config.radius = rover.g2.turn_radius;
-        gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)rover.g2.turn_radius);
+    if (config.radius < g2.turn_radius) {
+        config.radius = g2.turn_radius;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)g2.turn_radius);
     }
 }

Rover/mode_guided.cpp

@@ -443,5 +443,5 @@ bool ModeGuided::limit_breached() const
 // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
 bool ModeGuided::use_scurves_for_navigation() const
 {
-    return ((rover.g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0);
+    return ((g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0);
 }

Rover/mode_loiter.cpp

@@ -23,16 +23,16 @@ void ModeLoiter::update()
     // get distance (in meters) to destination
     _distance_to_destination = rover.current_loc.get_distance(_destination);
 
-    const float loiter_radius = rover.g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
+    const float loiter_radius = g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
 
     // if within loiter radius slew desired speed towards zero and use existing desired heading
     if (_distance_to_destination <= loiter_radius) {
         // sailboats should not stop unless motoring
-        const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
+        const float desired_speed_within_radius = g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
 
         // if we have a sail but not trying to use it then point into the wind
-        if (!rover.g2.sailboat.tack_enabled() && rover.g2.sailboat.sail_enabled()) {
+        if (!g2.sailboat.tack_enabled() && g2.sailboat.sail_enabled()) {
             _desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f;
         }
     } else {

Tools/AP_Bootloader/board_types.txt

@@ -103,6 +103,8 @@ AP_HW_JDMINIF405                      144
 AP_HW_KAKUTEF7_MINI                   145
 AP_HW_H757I_EVAL                      146
 AP_HW_F4BY                             20 # value due to previous release by vendor
+AP_HW_MAZZYSTARDRONE                  188
+
 AP_HW_VRBRAIN_V51                    1151
 AP_HW_VRBRAIN_V52                    1152
 AP_HW_VRBRAIN_V54                    1154
@@ -111,7 +113,7 @@ AP_HW_VRUBRAIN_V51                   1351
 AP_HW_F103_PERIPH                    1000
 AP_HW_CUAV_GPS                       1001
 AP_HW_OMNIBUSF4                      1002
-AP_HW_CUBEBLACK+                     1003
+AP_HW_CUBEBLACKPLUS                  1003
 AP_HW_F303_PERIPH                    1004
 AP_HW_ZUBAXGNSS                      1005
 AP_HW_NIGHTCRAWLER                   1006
@@ -200,7 +202,7 @@ AP_HW_HolybroCompass                 1088
 AP_HW_FOXEERH743_V1                  1089
 AP_HW_PixFlamingoL4R5_V1             1090
 
-AP_HW_Sierra-TrueNav Pro             1091
+AP_HW_Sierra-TrueNavPro              1091
 AP_HW_Sierra-TrueNav                 1092
 AP_HW_Sierra-TrueNorth               1093
 AP_HW_Sierra-TrueSpeed               1094
@@ -287,6 +289,8 @@ AP_HW_FlywooF405HD_AIOv2             1180
 AP_HW_ESP32_PERIPH                   1205
 AP_HW_ESP32S3_PERIPH                 1206
 
+AP_HW_KHA_ETH                        1315
+
 AP_HW_CUBEORANGE_PERIPH              1400
 AP_HW_CUBEBLACK_PERIPH               1401
 AP_HW_PIXRACER_PERIPH                1402

Tools/ardupilotwaf/boards.py

@@ -187,7 +187,7 @@ def srcpath(path):
         cfg.env.prepend_value('INCLUDES', [
             cfg.srcnode.find_dir('libraries/AP_Common/missing').abspath()
         ])
-        if os.path.exists(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json')):
+        if os.path.exists(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json')) and 'AP_NO_COMPILE_COMMANDS' not in os.environ:
             # change c_cpp_properties.json configure the VSCode Intellisense env
             c_cpp_properties = json.load(open(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json')))
             for config in c_cpp_properties['configurations']:
@@ -944,6 +944,19 @@ def configure_env(self, cfg, env):
             HAL_PERIPH_ENABLE_GPS = 1,
         )
 
+class sitl_periph_battmon(sitl_periph):
+    def configure_env(self, cfg, env):
+        cfg.env.AP_PERIPH = 1
+        super(sitl_periph_battmon, self).configure_env(cfg, env)
+        env.DEFINES.update(
+            HAL_BUILD_AP_PERIPH = 1,
+            PERIPH_FW = 1,
+            CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_battmon"',
+            APJ_BOARD_ID = 101,
+
+            HAL_PERIPH_ENABLE_BATTERY = 1,
+        )
+
 class esp32(Board):
     abstract = True
     toolchain = 'xtensa-esp32-elf'