#include <mavlink2.h>
#include <AccelStepper.h>
// Define system and component IDs
#define system_id 5 // Arduino sysid
#define component_id 158 // Arduino compid
#define type MAV_TYPE_ANTENNA_TRACKER
#define autopilot MAV_AUTOPILOT_ARDUPILOTMEGA
// Define pin connections for TPU6600 driver
#define Pan_DIR_PIN PB5
#define Pan_PUL_PIN PB4
#define Tilt_DIR_PIN PB9
#define Tilt_PUL_PIN PB8
#define PIN_AutopilotSerial_TX PA9
#define PIN_AutopilotSerial_RX PA10
#define up_sensor_pin PA15
#define down_sensor_pin PA12
// Stepper motor objects
AccelStepper Panstepper(1, Pan_PUL_PIN, Pan_DIR_PIN); // (1) means driver is connected to a TPU6600
AccelStepper Tiltstepper(1, Tilt_PUL_PIN, Tilt_DIR_PIN);
// RX TX
HardwareSerial AutopilotSerial(PIN_AutopilotSerial_RX, PIN_AutopilotSerial_TX); // Autopilot
// Variables for MAVLink communication
int received_sysid; // Pixhawk sysid
int received_compid;
float pitch, yaw;
// Limits for motor movement
int Pan__min = -3200;
int Pan_max = 3200;
int Tilt_min = -2400;
int Tilt_max = 2400;
// Channels for servo input
int Pan_ch;
int Tilt_ch;
float factor_Pan, factor_Tilt;
// Gearbox and pulse settings
int PulsePerRev = 3200;
int FirstGearboxRatio = 80;
int SecondGearboxRatio = 5;
int FirstGearboxRatioPan = 80;
int SecondGearboxRatioPan = 5;
// Desired angles and errors
float Desired_Pan, Desired_Pan_Speed, Desired_yaw, yaw_error, stepsToMovePan_speed;
float Desired_Tilt, Desired_Tilt_Speed, Desired_pitch, Pitch_error, stepsToMoveTilt_speed;
// Steps to move motors
long stepsToMovePan;
long stepsToMoveTilt;
// Calibration and mode flags
int cal_enable = 0;
int mode;
// Navigation angles
float pitch_nav, yaw_nav;
// Time variables
float start_time, current_time;
// Sensor status
int up_sensor;
int down_sensor;
int limit_sensors = 0;
// PID constants
float Kp = 0.5;
float Ki = 0.2;
float Kd = 0.1;
// PID variables
double yaw_derivative, yaw_proportional, yaw_error_output;
double Pitch_derivative, Pitch_proportional, Pitch_error_output;
double yaw_integral = 0;
double Pitch_integral = 0;
double yaw_last_error = 0;
double Pitch_last_error = 0;
// Setup function
void setup () {
// Initialize stepper motor parameters
Panstepper.setMaxSpeed(3200); // Speed = Steps / second
Panstepper.setAcceleration(700);
Panstepper.setSpeed(1600);
delay(500);
Tiltstepper.setMaxSpeed(2400); // Speed = Steps / second
Tiltstepper.setAcceleration(700);
Tiltstepper.setSpeed(1600);
delay(500);
// Set sensor control pins to input
pinMode(up_sensor_pin, INPUT);
pinMode(down_sensor_pin, INPUT);
// Initialize MAVLink communication
mav_begin();
Stream();
}
// Main loop function
void loop () {
// Read MAVLink messages
read_mavlink();
// Check mode and tilt channel
if (mode == 10 && (Tilt_ch > 0)) {
// Auto control mode
if (Pan_ch > 1400 && Pan_ch < 1600){
Desired_Pan_Speed = map(Pan_ch, 1100, 1900, Pan__min, Pan_max) * 10;
Serial.println(Desired_Pan_Speed);
} else {
Desired_Pan_Speed = map(Pan_ch, 1100, 1900, Pan__min, Pan_max) * 5;
}
if (Tilt_ch > 1400 && Tilt_ch < 1600){
Desired_Tilt_Speed = map(Tilt_ch, 1100, 1900, Tilt_min, Tilt_max) * -10;
} else {
Desired_Tilt_Speed = map(Tilt_ch, 1100, 1900, Tilt_min, Tilt_max) * -5;
}
// Move the stepper motor to the desired position
if (pitch <= 43 && pitch >= -43){
Tiltstepper.setSpeed(Desired_Tilt_Speed);
Tiltstepper.runSpeed();
} else if(pitch < -43 && Desired_Tilt_Speed < 0){
Tiltstepper.setSpeed(Desired_Tilt_Speed);
Tiltstepper.runSpeed();
} else if(pitch > 43 && Desired_Tilt_Speed > 0){
Tiltstepper.setSpeed(Desired_Tilt_Speed);
Tiltstepper.runSpeed();
}
Panstepper.setSpeed(Desired_Pan_Speed);
Panstepper.runSpeed();
} else {
// Manual control mode
if (Pan_ch == 0 || Tilt_ch == 0){
Desired_Pan = 0;
Desired_Tilt = 0;
}
else {
Desired_Pan = map(Pan_ch, 1100, 1900, Pan__min, Pan_max);
Desired_Tilt = map(Tilt_ch, 1100, 1900, Tilt_min, Tilt_max);
}
// Move the stepper motor to the desired position
if (pitch <= 43 && pitch >= -43){
Tiltstepper.setSpeed(Desired_Tilt);
Tiltstepper.runSpeed();
} else if(pitch < -43 && Desired_Tilt < 0){
Tiltstepper.setSpeed(Desired_Tilt);
Tiltstepper.runSpeed();
} else if(pitch > 43 && Desired_Tilt > 0){
Tiltstepper.setSpeed(Desired_Tilt);
Tiltstepper.runSpeed();
}
Panstepper.setSpeed(Desired_Pan);
Panstepper.runSpeed();
}
}
// Function to read MAVLink messages
void read_mavlink() {
while (AutopilotSerial.available() > 0) {
mavlink_message_t msg;
mavlink_status_t status1;
uint8_t ch = AutopilotSerial.read();
if (mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status1)) {
if (msg.sysid == 5) {
switch (msg.msgid) {
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
// Code for handling servo output raw message
mavlink_servo_output_raw_t data;
mavlink_msg_servo_output_raw_decode(&msg, &data);
Pan_ch = (data.servo1_raw);
Tilt_ch = (data.servo2_raw);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
// Code for handling attitude message
mavlink_attitude_t data;
mavlink_msg_attitude_decode(&msg, &data);
pitch = degrees(data.pitch);
yaw = degrees(data.yaw);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Code for handling heartbeat message
mavlink_heartbeat_t data;
mavlink_msg_heartbeat_decode(&msg, &data);
mode = (data.custom_mode);
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
// Code for handling navigation controller output message
mavlink_nav_controller_output_t data;
mavlink_msg_nav_controller_output_decode(&msg, &data);
pitch_nav = (data.nav_pitch);
yaw_nav = (data.nav_bearing);
break;
}
}
}
}
}
}
// Function to initialize MAVLink communication
bool mav_begin() {
AutopilotSerial.begin(115200);
if (AutopilotSerial.available() <= 0) {
return false;
} else {
return true;
}
}
// At first we will send some HeartBeats to Pixhawk to check whether it's available or not??
// After that we will check for whether we are recieving HeartBeats back from Pixhawk if Yes,
// We will note down its sysid and compid to send it a req to Stream Data.
void Stream(){
delay(2000);
int flag=1;
// Serial.println("Sending Heartbeats...");
mavlink_message_t msghb;
mavlink_heartbeat_t heartbeat;
uint8_t bufhb[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_heartbeat_pack(system_id, component_id, &msghb, type, autopilot, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);
uint16_t lenhb = mavlink_msg_to_send_buffer(bufhb, &msghb);
delay(1000);
AutopilotSerial.write(bufhb,lenhb);
// Serial.println("Heartbeats sent! Now will check for recieved heartbeats to record sysid and compid...");
// Looping untill we get the required data.
while(flag==1){
delay(1);
while(AutopilotSerial.available() > 0){
mavlink_message_t msgpx;
mavlink_status_t statuspx;
uint8_t ch = AutopilotSerial.read();
if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msgpx, &statuspx)){
// Serial.println("Message Parsing Done!");
switch(msgpx.msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(&msgpx, &packet);
received_sysid = msgpx.sysid; // Pixhawk sysid
received_compid = msgpx.compid; // Pixhawk compid
// Serial.println("sysid and compid successfully recorded");
flag = 0;
break;
}
}
}
}
}
// Sending request for data stream...
// Serial.println("Now sending request for data stream...");
delay(2000);
mavlink_message_t msgds;
uint8_t bufds[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_request_data_stream_pack(system_id, component_id, &msgds, received_sysid, received_compid, MAV_DATA_STREAM_RC_CHANNELS , 0x05, 1);
uint16_t lends = mavlink_msg_to_send_buffer(bufds, &msgds);
delay(1000);
AutopilotSerial.write(bufds,lends);
Serial.println("Request sent! Now you are ready to recieve datas...");
}
// Function to wrap angle between -180 to 180
float wrap_180(float angle) {
while (angle >= 180) {
angle -= 360;
}
while (angle < 180) {
angle = angle;
}
return angle;
}
float wrap_360_cd(float angle)
{
while (angle > -180 && angle<=0) {
angle += 360;
}
while (angle <= 180 && angle>0) {
angle = angle;
}
return angle;
}
Loading
stm32-bluepill
stm32-bluepill