Code for drone im working on

can somebody make sure i can use this code for a self learning drone with a personality that follows you around?

#include <px4_posix.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/gps_position.h>
#include <px4_platform_common/log.h>
#include
#include <px4_platform_common/module.h>

// Dummy class for GPS target tracking (You or the phone providing GPS data)
class TargetGPS {
public:
TargetGPS() : target_lat(0.0), target_lon(0.0), target_alt(0.0) {}

void setPosition(double lat, double lon, double alt) {
    target_lat = lat;
    target_lon = lon;
    target_alt = alt;
}

double getLatitude() const { return target_lat; }
double getLongitude() const { return target_lon; }
double getAltitude() const { return target_alt; }

private:
double target_lat;
double target_lon;
double target_alt;
};

// Main drone control class
class DroneControlModule {
public:
DroneControlModule() : _vehicle_command_sub(nullptr), _vehicle_status_pub(nullptr), _gps_sub(nullptr) {
_personality = “cheerful”; // Default personality
}

int init() {
    // Initialize the subscription and publication
    _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
    _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
    _gps_sub = orb_subscribe(ORB_ID(gps_position));

    return 0;
}

void armDrone() {
    // Arm the drone and express mood
    PX4_INFO("Yay! I'm ready to fly!");
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
    cmd.param1 = 1.0f; // Arm the drone (1.0 = arm, 0.0 = disarm)
    cmd.param2 = 0.0f; // No extra parameters
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void takeoff() {
    // Take off and express mood
    PX4_INFO("Taking off! Let's soar!");
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
    cmd.param1 = 10.0f; // Altitude for takeoff in meters
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void followTarget() {
    gps_position_s gps_data;
    orb_copy(ORB_ID(gps_position), _gps_sub, &gps_data);

    // Assuming we have a method of getting the target's position (you or your phone)
    double target_lat = _target_gps.getLatitude();
    double target_lon = _target_gps.getLongitude();
    double target_alt = _target_gps.getAltitude();

    // Calculate the relative position of the drone
    double delta_lat = target_lat - gps_data.lat;
    double delta_lon = target_lon - gps_data.lon;
    double delta_alt = target_alt - gps_data.alt;

    // Adjust drone position to follow the target
    if (abs(delta_lat) > 0.0001 || abs(delta_lon) > 0.0001) {
        PX4_INFO("Following target...");

        vehicle_command_s cmd;
        cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT;
        cmd.param1 = 0.0f; // Radius (can be set for waypoint radius)
        cmd.param2 = target_lat;
        cmd.param3 = target_lon;
        cmd.param4 = target_alt;
        orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
    }
}

void land() {
    PX4_INFO("Landing now...");
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
    cmd.param1 = 0.0f; // No additional parameters
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

private:
orb_subscription_t _vehicle_command_sub;
orb_advertise_t _vehicle_status_pub;
orb_subscription_t _gps_sub;
vehicle_status_s _vehicle_status;
std::string _personality; // Store the personality type

TargetGPS _target_gps; // Target GPS (you or the device you want to follow)

};

// Main entry for PX4 module
extern “C” int px4_main(int argc, char *argv) {
DroneControlModule drone;

if (drone.init() != 0) {
    PX4_ERR("Failed to initialize drone control module");
    return -1;
}

// Arm the drone and take off
drone.armDrone();
sleep(2); // Allow some time for arming
drone.takeoff();
sleep(10); // Wait for takeoff

// Start following the target
while (true) {
    drone.followTarget();
    sleep(1);  // Update every second
}

return 0;

}

PX4_MODULE

update to code:removed magnetic charging and added voice command

#include <px4_posix.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/gps_position.h>
#include <px4_platform_common/log.h>
#include
#include <px4_platform_common/module.h>
#include <mavlink.h>

class DroneControlModule {
public:
DroneControlModule() : _vehicle_command_sub(nullptr), _vehicle_status_pub(nullptr), _battery_sub(nullptr), _gps_sub(nullptr) {
_personality = “neutral”; // Default personality
}

int init() {
    // Initialize the subscription and publication
    _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
    _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
    _battery_sub = orb_subscribe(ORB_ID(battery_status));
    _gps_sub = orb_subscribe(ORB_ID(gps_position));

    return 0;
}

void updatePersonality() {
    // Check battery status to adjust drone personality (mood)
    battery_status_s battery_data;
    orb_copy(ORB_ID(battery_status), _battery_sub, &battery_data);

    if (battery_data.remaining < 20.0f) {
        // Low battery
        _personality = "worried";
        PX4_INFO("I'm worried. Battery is low!");
    } else if (battery_data.remaining < 60.0f) {
        // Medium battery
        _personality = "neutral";
        PX4_INFO("I'm feeling neutral. Battery is medium.");
    } else {
        // High battery
        _personality = "cheerful";
        PX4_INFO("I'm happy! Battery is full!");
    }
}

void armDrone() {
    PX4_INFO("Yay! I'm ready to fly! My mood: %s", _personality.c_str());
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
    cmd.param1 = 1.0f; // Arm the drone (1.0 = arm, 0.0 = disarm)
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void takeoff() {
    PX4_INFO("Taking off! My mood: %s", _personality.c_str());
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
    cmd.param1 = 10.0f; // Altitude for takeoff in meters
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void land() {
    PX4_INFO("Landing now! My mood: %s", _personality.c_str());
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void hover() {
    PX4_INFO("Hovering in place. My mood: %s", _personality.c_str());
    vehicle_command_s cmd;
    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT;
    orb_publish(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
}

void handleVoiceCommand(const std::string &command) {
    if (command == "land") {
        land();
    } else if (command == "take off") {
        takeoff();
    } else if (command == "hover") {
        hover();
    }
}

void handleMavlinkCommand(mavlink_message_t &msg) {
    if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
        mavlink_command_long_t command_long;
        mavlink_msg_command_long_decode(&msg, &command_long);

        if (command_long.command == MAV_CMD_NAV_LAND) {
            land();
        } else if (command_long.command == MAV_CMD_NAV_TAKEOFF) {
            takeoff();
        } else if (command_long.command == MAV_CMD_NAV_WAYPOINT) {
            hover();
        }
    }
}

private:
orb_subscription_t _vehicle_command_sub;
orb_advertise_t _vehicle_status_pub;
orb_subscription_t _battery_sub;
orb_subscription_t _gps_sub;
vehicle_status_s _vehicle_status;
std::string _personality; // Mood of the drone
};

// Main entry for PX4 module
extern “C” int px4_main(int argc, char *argv) {
DroneControlModule drone;

if (drone.init() != 0) {
    PX4_ERR("Failed to initialize drone control module");
    return -1;
}

// Arm the drone and take off
drone.armDrone();
sleep(2); // Allow some time for arming
drone.takeoff();
sleep(10); // Wait for takeoff

// Simulate voice commands to control the drone
// For example, simulate a landing command after 15 seconds
sleep(15);
drone.handleVoiceCommand("land");

// Main loop to handle MAVLink messages
mavlink_message_t msg;
while (true) {
    // Check for MAVLink message and handle
    if (mavlink_parse_char(MAVLINK_COMM_0, msg, NULL)) {
        drone.handleMavlinkCommand(msg);
    }

    // Update personality based on battery level
    drone.updatePersonality();

    // Sleep for 1 second to simulate time passing
    sleep(1);
}

return 0;

}

PX4_MODULE

1 Like

How can i contact you personally. I need some codes to move my camera gimbal using PX4 v1.15

Did you place this code as a file in the px4 module folder. Are you using this based on Px4 v1.15 firmware