post at ... http://diydrones.com/group/sailing-drone?commentId=705844%3AComment%3A1448717&xg_source=msg_com_group
here is a youtube video of my boat ... https://www.youtube.com/watch?v=cSWdFh2meLo
:::::::::: I first made a copy of ArduPlane that I called ArduPlanePete.
:::::::::: I got this to compile and run.
::::::::::: Then in the main body of ArduPlanePete I made these two changes
1.
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <Arduino.h> //// added by Pete
#include <APM_sailing> //// added by Pete
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
2.
{ update_flight_mode, 1, 1000 },
{ stabilize, 1, 3200 },
// { set_servos, 1, 1100 }, // pete comments out this line for total servo control
{ update_GPS, 5, 4000 },
{ navigate, 5, 4800 },
{ update_compass, 5, 1500 },
{ read_airspeed, 5, 1500 },
{ read_control_switch, 15, 1000 },
{ update_alt, 5, 3400 },
{ calc_altitude_error, 5, 1000 },
{ update_commands, 5, 7000 },
{ obc_fs_check, 5, 1000 },
{ gcs_update, 1, 1700 },
{ gcs_data_stream_send, 2, 3000 },
{ update_mount, 1, 1500 },
{ update_events, 15, 1500 },
{ check_usb_mux, 5, 1000 },
{ read_battery, 5, 1000 },
{ compass_accumulate, 1, 1500 },
{ barometer_accumulate, 1, 900 },
{ one_second_loop, 50, 3900 },
{ sailing, 50, 3900 }, // added by pete
{ update_logging, 5, 1000 },
{ read_receiver_rssi, 5, 1000 },
::::::::::::: I used config.h for all the sailing variables
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// pete test
///////////// sail stuff ////////////////
int wind_direction; // one side of the boat or the other
int previous_wind_direction;
int current_wind_direction_count = 0;
int wind_direction_before_tack;
int after_tack_heading;
int off_heading;
int off_course;
int previous_off_course;
int off_wind = 150;
int heel;
int current_boat_heading;
int desired_boat_heading;
int a_roll; // absolute value of roll sensor
float pete_gps_course;
int pete_ground_course_d;
float pete_gps_speed;
float max_speed = 0;
int manual_tack_counter;
bool tacking;
bool wind_shift;
int front_sail_servo_center = 1500;
int rear_sail_servo_center = 1500;
int front_sail_servo = 1500;
int rear_sail_servo = 1500;
int tack_bump = 70;
int tack_counter;
int sailing_counter = 0;
:::::::: Below is the file I call APM_sailing
:::::::::: I did a "tab" add to ArduPlanePete for APM_sailing to build this file
//////////// start of pete patch for total servo control
void multiread(AP_HAL::RCInput* in, uint16_t* channels) {
/* Multi-channel read method: */
uint8_t valid;
valid = in->read(channels, 8);
/*
hal.console->printf_P(
PSTR("multi read %d: %d %d %d %d %d %d %d %d\r\n"),
(int) valid,
channels[0], channels[1], channels[2], channels[3],
channels[4], channels[5], channels[6], channels[7]);
*/
}
void multiwrite(AP_HAL::RCOutput* out, uint16_t* channels) {
out->write(0, channels, 8);
/* Upper channels duplicate lower channels*/
out->write(8, channels, 8);
}
///////////////////////////////////////////// end pete patch
void sailing(void)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
// read the 6 ch from the RC
uint16_t channels[8];
hal.gpio->write(27, 1);
multiread(hal.rcin, channels);
// float gps_heading = ToRad(ground_course_cd * 0.01f);
// float gps_speed = ground_speed_cm * 0.01f;
// float sin_heading, cos_heading;
// public info found in file... AP_GPS_HIL.cpp
pete_gps_course = g_gps->ground_course_cd; // it looks like the cd stands for 1/100 degrees
pete_ground_course_d = pete_gps_course / 100; // this works fine and gives numbers from 0 to 360
previous_pete_gps_speed = pete_gps_speed;
pete_gps_speed = g_gps->ground_speed_cm; /// pete test // 1 centimeter / second = 0.0223693629 miles per hour
if (max_speed < pete_gps_speed) {max_speed = pete_gps_speed;} // walking around the backhard my max speed was 277.
///////////////////////////////////////////////////////////////////////////
///////////// sail stuff ////////////////
//////////////////////////////////////////////////////////////////////////
/* not debuged
// force a tack to start or end... this is a pulse switch
if (channels[4] > 1400 && (manual_tack_counter > 5))
{
if (tacking){tacking = false;} else {tacking = true; tack_counter = 0; wind_direction_before_tack = wind_direction;}
}
manual_tack_counter++;
*/
sailing_counter++;
// tack every 200 seconds
int tack_check = sailing_counter / 200;
if (sailing_counter == (tack_check * 200)) {tacking = true; tack_counter = 0; wind_direction_before_tack = wind_direction;}
previous_wind_direction = wind_direction;
if(ahrs.roll_sensor < 0) {wind_direction = -1;} else {wind_direction = 1;} // that is one side of the boat or the other
if (previous_wind_direction == wind_direction){current_wind_direction_count++;} else {current_wind_direction_count = 0;}
a_roll = abs(ahrs.roll_sensor);
heel = a_roll / 100; // now heel is in degrees maybe
if (heel > 30){tacking = true; tack_counter = 0; wind_direction_before_tack = wind_direction;}
// if (wind_direction_before_tack -= wind_direction) {wind_shift = true;} // not sure what to do with this idea
if (tacking)
{
tack_counter++;
if (tack_counter > 5) {tack_bump = -100;} else {tack_bump = 100;}
front_sail_servo = front_sail_servo + (tack_bump * wind_direction_before_tack);
rear_sail_servo = rear_sail_servo - (tack_bump * wind_direction_before_tack);
}
// does not work right || ((current_wind_direction_count > 3) && (wind_direction_before_tack -= wind_direction)))
// force end of tack after 10 seconds
if (tack_counter > 10)
{
tacking = false;
tack_counter = 0;
front_sail_servo = front_sail_servo_center;
rear_sail_servo = rear_sail_servo_center;
// after_tack_heading = compass.mag_x; // still working on this idea
}
off_heading = 5;
off_wind = 10;
// if (previous_pete_gps_speed > pete_gps_speed)... do something like undo what just happened
/////////////// servo testing ///// just the first 20 seconds /////////
if (sailing_counter < 20)
{
off_heading = 0;
off_wind = 0;
front_sail_servo = sailing_counter * 100;
rear_sail_servo = sailing_counter * 100;
channels[0] = sailing_counter * 100;
channels[1] = sailing_counter * 100;
}
if (sailing_counter == 20)
{
front_sail_servo = front_sail_servo_center;
rear_sail_servo = rear_sail_servo_center;
}
////////////// end servo testing //////////////
////// stear the boat in a straight line
// need to add in heel and speed adjustment
// switch to gps heading as soon as a fix is taken
/* just testing maybe sample output to MP
gcs_send_text_fmt(PSTR("sailing %.2fV Used %.0f mAh"),
battery.voltage, battery.current_total_mah);
*/
// just for testing to see if I can see any effect on the servos
//////////////////////////////// if manual control /////////////////////////
if (channels[5] > 1400){ // use manual input ... this switch stays in position ... on or off
channels[3] = front_sail_servo + (4 * heel * wind_direction);
channels[2] = rear_sail_servo - (5 * heel * wind_direction);
// channels[3] = front_sail_servo - off_heading - (off_wind * wind_direction);
// channels[2] = rear_sail_servo + off_heading - (off_wind * wind_direction);
} //////////////////////////////// end of if manual control /////////////////////////
// write to servos
multiwrite(hal.rcout, channels);
} // end of sailing
:::::::::: to test this rock the boat side to side and the servos should move
Replies