Skip to content

Commit

Permalink
ハートモードの叩き作成
Browse files Browse the repository at this point in the history
  • Loading branch information
hamalt committed Jun 24, 2024
1 parent 066d206 commit c7db03b
Show file tree
Hide file tree
Showing 6 changed files with 189 additions and 1 deletion.
7 changes: 7 additions & 0 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -205,6 +205,7 @@ class Copter : public AP_Vehicle {
friend class ModeAvoidADSB;
friend class ModeBrake;
friend class ModeCircle;
friend class ModeHeart;
friend class ModeDrift;
friend class ModeFlip;
friend class ModeFlowHold;
Expand Down Expand Up @@ -677,6 +678,9 @@ class Copter : public AP_Vehicle {
#if MODE_CIRCLE_ENABLED == ENABLED
bool get_circle_radius(float &radius_m) override;
bool set_circle_rate(float rate_dps) override;
#endif
#if MODE_HEART_ENABLED == ENABLED
// TODO: for heart code?
#endif
bool set_desired_speed(float speed) override;
#if MODE_AUTO_ENABLED == ENABLED
Expand Down Expand Up @@ -987,6 +991,9 @@ class Copter : public AP_Vehicle {
#if MODE_CIRCLE_ENABLED == ENABLED
ModeCircle mode_circle;
#endif
#if MODE_HEART_ENABLED == ENABLED
ModeHeart mode_heart;
#endif
#if MODE_DRIFT_ENABLED == ENABLED
ModeDrift mode_drift;
#endif
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when pwm of Flightmode channel(FLTMODE_CH) is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL, 99:Myfirst
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate,27:Auto RTL, 99:Myfirst, 81:Heart
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),

Expand Down
6 changes: 6 additions & 0 deletions ArduCopter/config.h
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,12 @@
# define MODE_CIRCLE_ENABLED ENABLED
#endif

//////////////////////////////////////////////////////////////////////////////
// Heart - fly vehicle around a central point
#ifndef MODE_HEART_ENABLED
# define MODE_HEART_ENABLED ENABLED
#endif

//////////////////////////////////////////////////////////////////////////////
// Drift - fly vehicle in altitude-held, coordinated-turn mode
#ifndef MODE_DRIFT_ENABLED
Expand Down
7 changes: 7 additions & 0 deletions ArduCopter/mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
break;
#endif

#if MODE_HEART_ENABLED == ENABLED
case Mode::Number::HEART:
ret = &mode_heart;
break;
#endif

#if MODE_LOITER_ENABLED == ENABLED
case Mode::Number::LOITER:
ret = &mode_loiter;
Expand Down Expand Up @@ -207,6 +213,7 @@ bool Copter::gcs_mode_enabled(const Mode::Number mode_num)
{
// List of modes that can be blocked, index is bit number in parameter bitmask
static const uint8_t mode_list [] {
(uint8_t)Mode::Number::HEART,
(uint8_t)Mode::Number::MYFIRST,
(uint8_t)Mode::Number::STABILIZE,
(uint8_t)Mode::Number::ACRO,
Expand Down
30 changes: 30 additions & 0 deletions ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ class Mode {
AUTO_RTL = 27, // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
TURTLE = 28, // Flip over after crash
MYFIRST = 99, // My first flight mode
HEART = 81

// Mode number 127 reserved for the "drone show mode" in the Skybrush
// fork at https://github.com/skybrush-io/ardupilot
Expand Down Expand Up @@ -862,6 +863,35 @@ class ModeCircle : public Mode {
bool speed_changing = false; // true when the roll stick is being held to facilitate stopping at 0 rate
};

class ModeHeart : public Mode {

public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::HEART; }

bool init(bool ignore_checks) override;
void run() override;

bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; };
bool is_autopilot() const override { return true; }

protected:

const char *name() const override { return "HEART"; }
const char *name4() const override { return "HART"; }

uint32_t wp_distance() const override;
int32_t wp_bearing() const override;

private:

// Heart
bool speed_changing = false; // true when the roll stick is being held to facilitate stopping at 0 rate
};


class ModeDrift : public Mode {

Expand Down
138 changes: 138 additions & 0 deletions ArduCopter/mode_heart.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,138 @@
#include "Copter.h"
#include <AP_Mount/AP_Mount.h>

#if MODE_HEART_ENABLED == ENABLED

/*
* Init and run calls for circle flight mode
*/

// heart_init - initialise circle controller flight mode
bool ModeHeart::init(bool ignore_checks)
{
speed_changing = false;

// set speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

// initialise circle controller including setting the circle center based on vehicle speed
copter.circle_nav->init();

#if HAL_MOUNT_ENABLED
AP_Mount *s = AP_Mount::get_singleton();

// Check if the CIRCLE_OPTIONS parameter have roi_at_center
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)) {
return false;
}
// point at the ground:
circle_center.set_alt_cm(0, Location::AltFrame::ABOVE_TERRAIN);
s->set_roi_target(circle_center);
}
#endif

// set auto yaw circle mode
auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);

return true;
}

// heart_run - runs the circle flight mode
// should be called at 100hz or more
void ModeHeart::run()
{
// set speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

// Check for any change in params and update in real time
copter.circle_nav->check_param_change();

// pilot changes to circle rate and radius
// skip if in radio failsafe
if (!copter.failsafe.radio && copter.circle_nav->pilot_control_enabled()) {
// update the circle controller's radius target based on pilot pitch stick inputs
const float radius_current = copter.circle_nav->get_radius(); // circle controller's radius target, which begins as the circle_radius parameter
const float pitch_stick = channel_pitch->norm_input_dz(); // pitch stick normalized -1 to 1
const float nav_speed = copter.wp_nav->get_default_speed_xy(); // copter WP_NAV parameter speed
const float radius_pilot_change = (pitch_stick * nav_speed) * G_Dt; // rate of change (pitch stick up reduces the radius, as in moving forward)
const float radius_new = MAX(radius_current + radius_pilot_change,0); // new radius target

if (!is_equal(radius_current, radius_new)) {
copter.circle_nav->set_radius_cm(radius_new);
}

// update the orbicular rate target based on pilot roll stick inputs
// skip if using CH6 tuning knob for circle rate
if (g.radio_tuning != TUNING_CIRCLE_RATE) {
const float roll_stick = channel_roll->norm_input_dz(); // roll stick normalized -1 to 1

if (is_zero(roll_stick)) {
// no speed change, so reset speed changing flag
speed_changing = false;
} else {
const float rate = copter.circle_nav->get_rate(); // circle controller's rate target, which begins as the circle_rate parameter
const float rate_current = copter.circle_nav->get_rate_current(); // current adjusted rate target, which is probably different from _rate
const float rate_pilot_change = (roll_stick * G_Dt); // rate of change from 0 to 1 degrees per second
float rate_new = rate_current; // new rate target
if (is_positive(rate)) {
// currently moving clockwise, constrain 0 to 90
rate_new = constrain_float(rate_current + rate_pilot_change, 0, 90);

} else if (is_negative(rate)) {
// currently moving counterclockwise, constrain -90 to 0
rate_new = constrain_float(rate_current + rate_pilot_change, -90, 0);

} else if (is_zero(rate) && !speed_changing) {
// Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
rate_new = rate_pilot_change;
}

speed_changing = true;
copter.circle_nav->set_rate(rate_new);
}
}
}

// get pilot desired climb rate (or zero if in radio failsafe)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
return;
}

// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

// update the vertical offset based on the surface measurement
copter.surface_tracking.update_surface_offset();

copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
pos_control->update_z_controller();

// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}

uint32_t ModeHeart::wp_distance() const
{
return copter.circle_nav->get_distance_to_target();
}

int32_t ModeHeart::wp_bearing() const
{
return copter.circle_nav->get_bearing_to_target();
}

#endif

0 comments on commit c7db03b

Please sign in to comment.