[RESOLVED] Understanding function

Firmware/software/electronics
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

[RESOLVED] Understanding function

Post by RyanMco »

Hi guys, could you please elaborate for me what does the function go_to does? and how is struct planner initiated without initiating it in go_to function?
int go_to(const struct data_go_to* data)
{
int result = 0;
if (isInGroup(data->groupMask)) {
struct vec hover_pos = mkvec(data->x, data->y, data->z);
xSemaphoreTake(lockTraj, portMAX_DELAY);
float t = usecTimestamp() / 1e6;
result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
xSemaphoreGive(lockTraj);
}
return result;
}

why I need planner struct? what's the purpose of using struct hover_pos? exactly Im not understanding what this below row code does inside the function go_to :
plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t)


any help and good explanation how go to works? and how setpoints are updated over trajectory? how does the struct planner initiated and where it's initiated?

I know by debugger that when call to go to function by python then its enter to that go_to function ..but didn't understand what's going exactly there .. so much appreciate to hand me a good explanation on how it works!!!


thanks alot!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Understanding function

Post by arnaud »

Can you please precise where the code is and send a link to github when sending this kind of question, it would help us a lot not loosing too much time guessing and searching. As a side note the forum as a "code display" block that allows to copy-paste formatted code. It makes it much easier to read.

The planner struct is initialized by the planned-goto function: https://github.com/bitcraze/crazyflie-f ... ner.c#L165

I am not familiar with this code but as far as I understand looking at it, the planner structure is filled-up with the current planned trajectory piece (7-order polynomial for X,Y,Z,YAW) by the goto functions. Then, when the getSetpoint function is called at 1KHz, this planner structure is used to generate each setpoints: https://github.com/bitcraze/crazyflie-f ... vel.c#L212
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Understanding function

Post by RyanMco »

I almost understand you ..
but how in do p->state initiated ? I tried to see where do we initiate that struct p in order to do switch but didn't find ..any help? also what's the purpsoe of

Code: Select all

 struct traj_eval ev = plan_current_goal(&planner, t);
into this GitHub firmware code in function

Code: Select all

 void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
https://github.com/bitcraze/crazyflie-f ... vel.c#L212

we are sending to plan_current_goal the address of "planner" struct but where do we initiate it? you already said it will be initiated in function

Code: Select all

int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t)
but where? and p->state in function plan_current_goal(p, t) include "trash" so how do we do switch to p->state without initializing it? thanks alot !

what does the struct p include?!
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Understanding function

Post by RyanMco »

And can I sat that hover_pos is implicitly the same as the current setpoint? I mean it's equivalent to say that the struct hover_pos is treated as struct named setpoint? I say that because we can say that hover_pos over x (I mean we just looking at x coordination for simplifying the problem) is like:
setpoint.x=x1+state.x which x1 is a coordination of sent data parameter from goto, x1 is a setpoint that sent by goto function of python script.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Understanding function

Post by arnaud »

I am not sure to understand your question about p->state. p->state is initially set by plan_init() and in the case of goto it is set in the plan_go_to() function: https://github.com/bitcraze/crazyflie-f ... ner.c#L165

In this code, the planner object is global and change to it is protected by a semaphore. On one side the structure is set by high-level-commander packets using the plan function, plan_go_to() in case of a goto. On the other side the getSetpoint function uses the planner structure to generate the current setpoint.
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Understanding function

Post by RyanMco »

Hi ;
but in struct traj_eval setpoint = plan_current_goal(p, t); there's switch(p->state) inside function "plan_current_goal(p, t)" so how he does assign at the end after calling plan_current_goal and use p->state before initiating it? the called function plan_current_goal is using p->state inside switch operator before initializing p->state ..because we are just initiating p->state at the end but before we call plan_current_goal and we use there p->state which yet we didn't initiate it because we didn't arrive to the end of the function..weird!
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Understanding function

Post by RyanMco »

And where's the getSetpoint function that use planner? I didn't find it
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Understanding function

Post by arnaud »

I linked the getSetpoint in an earlier message: https://github.com/bitcraze/crazyflie-f ... vel.c#L212

The planner structure that is passed around is the global planner object shared by the whole high-level controller code, it is initialized in plan_init() and filled up when high-level commander packets are received.

Did you find any point in code where p->state does not point to the global planner structure declared there: https://github.com/bitcraze/crazyflie-f ... evel.c#L89 ?
RyanMco
Expert
Posts: 159
Joined: Tue Apr 09, 2019 6:15 am

Re: Understanding function

Post by RyanMco »

NOW I understand you very well, I didn't figure out that the struct planner is initiated by plan_init ! thanks ..
moreover, this function:

Code: Select all

struct traj_eval plan_current_goal(struct planner *p, float t)
{
	switch (p->state) {
		case TRAJECTORY_STATE_LANDING:
			if (plan_is_finished(p, t)) {
				p->state = TRAJECTORY_STATE_IDLE;
			}
			// intentional fall-thru
		case TRAJECTORY_STATE_FLYING:
			return plan_eval(p, t);

		default:
			return traj_eval_invalid();
	}
}
what does it do? what's the purpose of that function? I didn't understand its logic aside to trajectory that my code is using once getting packets.

there's also another function :

Code: Select all

int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t)
{
	// allow in any state, i.e., can also be used to take-off or land

	struct traj_eval setpoint = plan_current_goal(p, t);

	if (relative) {
		hover_pos = vadd(hover_pos, setpoint.pos);
		hover_yaw += setpoint.yaw;
	}
what does the struct hover_pos mean? is it the current setpoint that I get? and what is the purpose of "vadd(hover_pos, setpoint.pos)" ?

according to "relative" .. so I must send in my python script the parameter relative as true inside goto function? I mean goto( goal=[0,0,1],param,param,relative=true) ..right?

thanks
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Understanding function

Post by arnaud »

The plan_current_goal() function generated the required current setpoint to follow the trajectory. It is the one that generates setpoints at 1KHz, it does calculate the setpoints from the polynomials and the time in the current trajectory.

The plan_go_to() function generates the polynomials used by the plan_current_goal() function.

The relative argument is to tell the high level commander if you want a trajectory relative to the current position, or if the setpoint you provide needs to be interpreted as an absolute target. This is exactly what the code does: if you ask for a relative goto it uses the current position setpoint as a base and adds hover_pos.

What you chose to set the relative parameter to in python depends if you want to send a relative or an absolute position setpoint.
Post Reply