# Code for drone im working on

**URL:** <https://discuss.px4.io/t/code-for-drone-im-working-on/43330>\
**Category:** Pixhawk\
**Created:** [January 17, 2025, 4:29am UTC](https://discuss.px4.io/t/code-for-drone-im-working-on/43330 "2025-01-17T04:29:56Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![dahappybee](https://discuss.px4.io/letter_avatar_proxy/v4/letter/d/779978/32.png) [@dahappybee](https://discuss.px4.io/u/dahappybee)\
**Post date:** [January 17, 2025, 4:29am UTC](https://discuss.px4.io/t/code-for-drone-im-working-on/43330/1 "2025-01-17T04:29:56Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![dahappybee](https://discuss.px4.io/letter_avatar_proxy/v4/letter/d/779978/32.png) [@dahappybee](https://discuss.px4.io/u/dahappybee)\
**Post date:** [January 17, 2025, 4:50am UTC](https://discuss.px4.io/t/code-for-drone-im-working-on/43330/2 "2025-01-17T04:50:02Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![sibujacob](https://discuss.px4.io/user_avatar/discuss.px4.io/sibujacob/32/9673_2.png) [@sibujacob](https://discuss.px4.io/u/sibujacob)\
**Post date:** [January 31, 2025, 8:33am UTC](https://discuss.px4.io/t/code-for-drone-im-working-on/43330/3 "2025-01-31T08:33:21Z")

</div>

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

---

<div class="post-metadata">

**Author:** ![sibujacob](https://discuss.px4.io/user_avatar/discuss.px4.io/sibujacob/32/9673_2.png) [@sibujacob](https://discuss.px4.io/u/sibujacob)\
**Post date:** [January 31, 2025, 8:37am UTC](https://discuss.px4.io/t/code-for-drone-im-working-on/43330/4 "2025-01-31T08:37:58Z")

</div>

> [@dahappybee](#):
>
> #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  
> }
> 
> ```auto
> 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;
> 
> ```auto
> 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;
> 
> ```
> 
> }

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