void callback_turn_angle(const std_msgs::Float64ConstPtr& angle)
{
// std::stringstream ss;
// ss << "Turning by angle: " << angle->data << ". Accum= " << _turn_accum << ", Heading= " << _heading;
_turn_accum += angle->data;
while (_turn_accum > 45.0) {
_turn_accum -= 90.0;
_heading++;
if (_heading >= 3) _heading -= 4;
}
while (_turn_accum < -45.0) {
_turn_accum += 90.0;
_heading--;
if (_heading <= -2) _heading += 4;
}
// ss << "\nCorrected heading = " << _heading << ", new accum= " << _turn_accum << std::endl;
// ROS_ERROR("%s",ss.str().c_str());
std_msgs::Int8 msg;
msg.data = get_compass();
_pub_compass.publish(msg);
}
void start_scheduler(void) {
while (1) {
//timer interrupt
if (sampling_flag == 1) {
timer_rst();
if (curr_channel == 1) {
// used to indicate scheduler cycle in GPIO pin RA5
update_status(SYSTEM_START);
PORTAbits.RA5 = !PORTAbits.RA5;
} else if (curr_channel == 100) {
update_status(SYSTEM_GYRO);
done = gyro_task();
} else if (curr_channel == 200) {
update_status(SYSTEM_ACCELERO);
done = get_acceleration();
} else if (curr_channel == 300) {
update_status(SYSTEM_COMPASS);
done = get_compass();
// //done = Compass_test_single();
// } else if (curr_channel == 250) {
} else if (curr_channel == 400) {
update_status(SYSTEM_FUSION);
ComplementaryFilter();
fusion_final();
} else if (curr_channel == 700) {
update_status(SYSTEM_UART);
//done = uart_task();
}
} else {
//update status if a task terminate
if (done && (check_status() == SYSTEM_UART) && TXSTAbits.TRMT) {
update_status(SYSTEM_IDLE);
done = 0;
} else if (done && ((check_status() == SYSTEM_GYRO) ||
check_status() == SYSTEM_ACCELERO ||
check_status() == SYSTEM_COMPASS ||
check_status() == SYSTEM_FUSION
)) {
update_status(SYSTEM_IDLE);
done = 0;
}
}
}
}
void AP_AHRS_NavEKF::update(void)
{
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// if we have a compass set and GPS lock we can start the EKF
if (get_compass() && get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilterDynamic();
}
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
update_trig();
}
}
}
int main(int argc,char **argv)
{
int wind_dir=0,heading=0,desired_heading=0,heading_error=0,new_sail_pos,new_rudder_pos;
double pgain=1.7;
int id=0;
setup();
while(stop_running!=1)
{
heading=get_compass();
wind_dir=get_wind();
//el rumbo al siguiente waypoint
desired_heading=get_desired_heading();
//Revisamos si deberemos ceñir
desired_heading=check_tacking(wind_dir,heading,desired_heading);
//definimos la diferencia entre el rumbo deseado y el rumbo
heading_error=get_hdg_diff(heading,desired_heading);
//Definimos un sector de 10º de error
if(abs(heading_error)<5)
{
new_rudder_pos=0;
}
else
{
//Calcular la posicion del timón
new_rudder_pos = (int)(heading_error * pgain);
}
//Ajustamos los limites entre [-90;90]
if(new_rudder_pos<-90)
{
new_rudder_pos=-90;
}
else if(new_rudder_pos>90)
{
new_rudder_pos=90;
}
/*Traduce el timon, cuando está en [-90;+90] y lo queremos en [90;270]*/
if(new_rudder_pos<0)
{
set_rudder(-1*new_rudder_pos);
}
else
{
set_rudder(360-new_rudder_pos);
}
//Calcular la posicion de las escotas, [0;18;36;54;72], con viento menor a 180º
if(wind_dir<180)
{
if (wind_dir < 70)
new_sail_pos = 0;
else if (wind_dir < 80)
new_sail_pos = 18;
else if (wind_dir < 90)
new_sail_pos = 36;
else if (wind_dir < 110)
new_sail_pos = 54;
else
new_sail_pos = 72;
}
//Calcular la posicion de las escotas, [0;342;324;306;288], con viento mayor a 290º
else
{
if (wind_dir >= 290)
new_sail_pos = 0;
else if (wind_dir >= 280)
new_sail_pos = 342;
else if (wind_dir >= 270)
new_sail_pos = 324;
else if (wind_dir >= 250)
new_sail_pos = 306;
else
new_sail_pos = 288;
}
set_sail(new_sail_pos);
printf("Id: %d Delta Rumbo: %d Rumbo: %d Dirección Viento: %d Nueva Posicion Timon: %d Nueva Posicion Escotas: %d SAILABLE: %d\n",id,heading_error,heading,wind_dir,new_rudder_pos,new_sail_pos,SAILABLE);
/*Ciclo de 4 Hz, sleep de 1/4 segundo*/
usleep(250000);
id++;
}
return 0;
}
int main(int argc,char **argv)
{
int wind_dir=0,heading=0,desired_heading=0,heading_error=0,new_sail_pos,new_rudder_pos;
double pgain=1.7;
setup();
while(stop_running!=1)
{
heading=get_compass();
wind_dir=get_wind();
//the heading to the next waypoint
desired_heading=get_desired_heading();
//see if we need to tack
desired_heading=check_tacking(wind_dir,heading,desired_heading);
//work out how many degrees difference between our heading and desired heading
heading_error=get_hdg_diff(heading,desired_heading);
//set a 10 degree wide deadband
if(abs(heading_error)<5)
{
new_rudder_pos=0;
}
else
{
//calculate rudder position
new_rudder_pos = (int)(heading_error * pgain);
}
//limit us between -90 and +90
if(new_rudder_pos<-90)
{
new_rudder_pos=-90;
}
else if(new_rudder_pos>90)
{
new_rudder_pos=90;
}
/*translate rudder
currently -90 to 90 we want 270 to 90
and it needs to be flipped
90 = 270
-90 = 90
*/
if(new_rudder_pos<0)
{
set_rudder(-1*new_rudder_pos);
}
else
{
set_rudder(360-new_rudder_pos);
}
//calculate the sail position
if(wind_dir<180)
{
if (wind_dir < 70)
new_sail_pos = 0;
else if (wind_dir < 80)
new_sail_pos = 18;
else if (wind_dir < 90)
new_sail_pos = 36;
else if (wind_dir < 110)
new_sail_pos = 54;
else
new_sail_pos = 72;
}
else
{
if (wind_dir >= 290)
new_sail_pos = 0;
else if (wind_dir >= 280)
new_sail_pos = 342;
else if (wind_dir >= 270)
new_sail_pos = 324;
else if (wind_dir >= 250)
new_sail_pos = 306;
else
new_sail_pos = 288;
}
set_sail(new_sail_pos);
printf("Heading: %d Wind: %d Heading Error: %d Rudder position: %d Sail position: %d Sailable: %d\n",heading_error,heading,wind_dir,new_rudder_pos,new_sail_pos,SAILABLE);
}
return 0;
}
int main(int argc,char **argv)
{
int wind_dir=0,heading=0,desired_heading=0,heading_error=0,new_sail_pos,new_rudder_pos;
double pgain=1.7;
int id=0;
int abort=0;
setup(); //inicia la conexion con el simulador server
stop_running = abort;
while(abort!=1)
{
heading=get_compass();
wind_dir=get_simulator_wind();
//el rumbo al siguiente waypoint
desired_heading=get_desired_heading();
//Revisamos si deberemos ceñir
desired_heading=check_tacking(wind_dir,heading,desired_heading);
//definimos la diferencia entre el rumbo deseado y el rumbo
heading_error=get_hdg_diff(heading,desired_heading);
//Definimos un sector de 10º de error
if(abs(heading_error)<5)
{
new_rudder_pos=0;
}
else
{
//Calcular la posicion del timón
new_rudder_pos = (int)(heading_error * pgain);
}
//Ajustamos los limites entre [-90;90]
if(new_rudder_pos<-90)
{
new_rudder_pos=-90;
}
else if(new_rudder_pos>90)
{
new_rudder_pos=90;
}
/*Traduce el timon, cuando está en [-90;+90] y lo queremos en [90;270]*/
if(new_rudder_pos<0)
{
set_rudder(-1*new_rudder_pos);
}
else
{
set_rudder(360-new_rudder_pos);
}
set_sail(calculate_sail_pos(wind_dir,new_sail_pos));
printf("Id: %d Delta Rumbo: %d Rumbo: %d Dirección Viento: %d Nueva Posicion Timon: %d Nueva Posicion Escotas: %d SAILABLE: %d\n",id,heading_error,heading,wind_dir,new_rudder_pos,new_sail_pos,SAILABLE);
usleep(250000); /*Ciclo de 4 Hz*/
stop_running = abort;
id++;
}
return 0;
}
void connect_compass_callback(const ros::SingleSubscriberPublisher& pub)
{
std_msgs::Int8 msg;
msg.data = get_compass();
pub.publish(msg);
}
